Merge http://linux-sound.bkbits.net/linux-sound

into home.transmeta.com:/home/torvalds/v2.5/linux

arch/i386/kernel/entry.S

@@ -508,6 +508,15 @@ debug_stack_correct:
 ENTRY(nmi)
 	cmpl $sysenter_entry,(%esp)
 	je nmi_stack_fixup
+	pushl %eax
+	movl %esp,%eax
+	/* Do not access memory above the end of our stack page,
+	 * it might not exist.
+	 */
+	andl $0x1fff,%eax
+	cmpl $0x1fec,%eax
+	popl %eax
+	jae nmi_stack_correct
 	cmpl $sysenter_entry,12(%esp)
 	je nmi_debug_stack_check
 nmi_stack_correct:

arch/ppc/boot/lib/zlib.c

@@ -316,8 +316,9 @@ int inflateReset(
 }
 
 
-int inflateEnd(z)
-z_stream *z;
+int inflateEnd(
+	z_stream *z
+)
 {
   uLong c;
 
@@ -332,9 +333,10 @@ z_stream *z;
 }
 
 
-int inflateInit2(z, w)
-z_stream *z;
-int w;
+int inflateInit2(
+	z_stream *z,
+	int w
+)
 {
   /* initialize state */
   if (z == Z_NULL)
@@ -378,8 +380,9 @@ int w;
 }
 
 
-int inflateInit(z)
-z_stream *z;
+int inflateInit(
+	z_stream *z
+)
 {
   return inflateInit2(z, DEF_WBITS);
 }
@@ -1128,8 +1131,9 @@ local int inflate_addhistory(
  * At the end of a Deflate-compressed PPP packet, we expect to have seen
  * a `stored' block type value but not the (zero) length bytes.
  */
-local int inflate_packet_flush(s)
-    inflate_blocks_statef *s;
+local int inflate_packet_flush(
+	inflate_blocks_statef *s
+)
 {
     if (s->mode != LENS)
 	return Z_DATA_ERROR;
@@ -1592,9 +1596,10 @@ local int inflate_trees_fixed(
 }
 
 
-local int inflate_trees_free(t, z)
-inflate_huft *t;        /* table to free */
-z_stream *z;            /* for zfree function */
+local int inflate_trees_free(
+	inflate_huft *t,        /* table to free */
+	z_stream *z             /* for zfree function */
+)
 /* Free the malloc'ed tables built by huft_build(), which makes a linked
    list of the tables it made, with the links in a dummy first entry of
    each table. */

arch/ppc64/boot/zlib.c

@@ -339,8 +339,9 @@ int inflateReset(
 }
 
 
-int inflateEnd(z)
-z_stream *z;
+int inflateEnd(
+	z_stream *z
+)
 {
   uLong c;
 
@@ -355,9 +356,10 @@ z_stream *z;
 }
 
 
-int inflateInit2(z, w)
-z_stream *z;
-int w;
+int inflateInit2(
+	z_stream *z,
+	int w
+)
 {
   /* initialize state */
   if (z == Z_NULL)
@@ -401,8 +403,9 @@ int w;
 }
 
 
-int inflateInit(z)
-z_stream *z;
+int inflateInit(
+	z_stream *z
+)
 {
   return inflateInit2(z, DEF_WBITS);
 }
@@ -1151,8 +1154,9 @@ local int inflate_addhistory(
  * At the end of a Deflate-compressed PPP packet, we expect to have seen
  * a `stored' block type value but not the (zero) length bytes.
  */
-local int inflate_packet_flush(s)
-    inflate_blocks_statef *s;
+local int inflate_packet_flush(
+	inflate_blocks_statef *s
+)
 {
     if (s->mode != LENS)
 	return Z_DATA_ERROR;
@@ -1615,9 +1619,10 @@ local int inflate_trees_fixed(
 }
 
 
-local int inflate_trees_free(t, z)
-inflate_huft *t;        /* table to free */
-z_stream *z;            /* for zfree function */
+local int inflate_trees_free(
+	inflate_huft *t,        /* table to free */
+	z_stream *z             /* for zfree function */
+)
 /* Free the malloc'ed tables built by huft_build(), which makes a linked
    list of the tables it made, with the links in a dummy first entry of
    each table. */

arch/sparc64/kernel/sys_sparc.c

@@ -223,9 +223,15 @@ asmlinkage int sys_ipc (unsigned call, int first, int second, unsigned long thir
 		}
 	if (call <= SHMCTL) 
 		switch (call) {
-		case SHMAT:
-			err = sys_shmat (first, (char *) ptr, second, (ulong *) third);
+		case SHMAT: {
+			ulong raddr;
+			err = sys_shmat (first, (char *) ptr, second, &raddr);
+			if (!err) {
+				if (put_user(raddr, (ulong __user *) third))
+					err = -EFAULT;
+			}
 			goto out;
+		}
 		case SHMDT:
 			err = sys_shmdt ((char *)ptr);
 			goto out;

drivers/net/ppp_generic.c

@@ -682,8 +682,10 @@ static int ppp_ioctl(struct inode *inode, struct file *file,
 			if (code == 0)
 				break;
 			err = -EFAULT;
-			if (copy_from_user(code, uprog.filter, len))
+			if (copy_from_user(code, uprog.filter, len)) {
+				kfree(code);
 				break;
+			}
 			err = sk_chk_filter(code, uprog.len);
 			if (err) {
 				kfree(code);

drivers/video/Makefile

@@ -55,7 +55,7 @@ obj-$(CONFIG_FB_PMAGB_B)          += pmagb-b-fb.o cfbfillrect.o cfbcopyarea.o cf
 obj-$(CONFIG_FB_MAXINE)           += maxinefb.o cfbfillrect.o cfbcopyarea.o cfbimgblt.o
 obj-$(CONFIG_FB_TX3912)           += tx3912fb.o cfbfillrect.o cfbcopyarea.o cfbimgblt.o
 
-obj-$(CONFIG_FB_MATROX)		  += matrox/
+obj-$(CONFIG_FB_MATROX)		  += matrox/ cfbfillrect.o cfbcopyarea.o cfbimgblt.o
 obj-$(CONFIG_FB_RIVA)		  += riva/ cfbimgblt.o vgastate.o 
 obj-$(CONFIG_FB_SIS)		  += sis/ cfbcopyarea.o cfbfillrect.o cfbimgblt.o
 obj-$(CONFIG_FB_ATY)		  += aty/ cfbcopyarea.o cfbfillrect.o cfbimgblt.o

drivers/video/matrox/i2c-matroxfb.c

@@ -113,7 +113,7 @@ static int i2c_bus_reg(struct i2c_bit_adapter* b, struct matrox_fb_info* minfo,
 	b->adapter = matrox_i2c_adapter_template;
 	snprintf(b->adapter.dev.name, DEVICE_NAME_SIZE, name,
 		minfo->fbcon.node);
-	b->adapter.data = b;
+	i2c_set_adapdata(&b->adapter, b);
 	b->adapter.algo_data = &b->bac;
 	b->bac = matrox_i2c_algo_template;
 	b->bac.data = b;

drivers/video/matrox/matroxfb_DAC1064.c

@@ -35,132 +35,11 @@
 #define DAC1064_OPT_MDIV2	0x00
 #define DAC1064_OPT_RESERVED	0x10
 
-static void matroxfb_DAC1064_flashcursor(unsigned long ptr) {
-	unsigned long flags;
-
-#define minfo ((struct matrox_fb_info*)ptr)
-	matroxfb_DAC_lock_irqsave(flags);
-	outDAC1064(PMINFO M1064_XCURCTRL, inDAC1064(PMINFO M1064_XCURCTRL) ^ M1064_XCURCTRL_DIS ^ M1064_XCURCTRL_XGA);
-	ACCESS_FBINFO(cursor.timer.expires) = jiffies + HZ/2;
-	add_timer(&ACCESS_FBINFO(cursor.timer));
-	matroxfb_DAC_unlock_irqrestore(flags);
-#undef minfo
-}
-
-static void matroxfb_DAC1064_createcursor(WPMINFO struct display* p) {
-	vaddr_t cursorbase;
-	u_int32_t xline;
-	unsigned int i;
-	unsigned int h, to;
-	CRITFLAGS
-
-	if (ACCESS_FBINFO(currcon_display) != p)
-		return;
-
-	matroxfb_createcursorshape(PMINFO p, p->var.vmode);
-
-	xline = (~0) << (32 - ACCESS_FBINFO(cursor.w));
-	cursorbase = ACCESS_FBINFO(video.vbase);
-	h = ACCESS_FBINFO(features.DAC1064.cursorimage);
-
-	CRITBEGIN
-
-#ifdef __BIG_ENDIAN
-	WaitTillIdle();
-	mga_outl(M_OPMODE, M_OPMODE_32BPP);
-#endif
-	to = ACCESS_FBINFO(cursor.u);
-	for (i = 0; i < to; i++) {
-		mga_writel(cursorbase, h, 0);
-		mga_writel(cursorbase, h+4, 0);
-		mga_writel(cursorbase, h+8, ~0);
-		mga_writel(cursorbase, h+12, ~0);
-		h += 16;
-	}
-	to = ACCESS_FBINFO(cursor.d);
-	for (; i < to; i++) {
-		mga_writel(cursorbase, h, 0);
-		mga_writel(cursorbase, h+4, xline);
-		mga_writel(cursorbase, h+8, ~0);
-		mga_writel(cursorbase, h+12, ~0);
-		h += 16;
-	}
-	for (; i < 64; i++) {
-		mga_writel(cursorbase, h, 0);
-		mga_writel(cursorbase, h+4, 0);
-		mga_writel(cursorbase, h+8, ~0);
-		mga_writel(cursorbase, h+12, ~0);
-		h += 16;
-	}
-#ifdef __BIG_ENDIAN
-	mga_outl(M_OPMODE, ACCESS_FBINFO(accel.m_opmode));
-#endif
-
-	CRITEND
-}
-
-static void matroxfb_DAC1064_cursor(struct display* p, int mode, int x, int y) {
-	unsigned long flags;
-	MINFO_FROM_DISP(p);
-
-	if (ACCESS_FBINFO(currcon_display) != p)
-		return;
-
-	if (mode == CM_ERASE) {
-		if (ACCESS_FBINFO(cursor.state) != CM_ERASE) {
-			del_timer_sync(&ACCESS_FBINFO(cursor.timer));
-			matroxfb_DAC_lock_irqsave(flags);
-			ACCESS_FBINFO(cursor.state) = CM_ERASE;
-			outDAC1064(PMINFO M1064_XCURCTRL, M1064_XCURCTRL_DIS);
-			matroxfb_DAC_unlock_irqrestore(flags);
-		}
-		return;
-	}
-	if ((p->conp->vc_cursor_type & CUR_HWMASK) != ACCESS_FBINFO(cursor.type))
-		matroxfb_DAC1064_createcursor(PMINFO p);
-	x *= fontwidth(p);
-	y *= fontheight(p);
-	y -= p->var.yoffset;
-	if (p->var.vmode & FB_VMODE_DOUBLE)
-		y *= 2;
-	del_timer_sync(&ACCESS_FBINFO(cursor.timer));
-	matroxfb_DAC_lock_irqsave(flags);
-	if ((x != ACCESS_FBINFO(cursor.x)) || (y != ACCESS_FBINFO(cursor.y)) || ACCESS_FBINFO(cursor.redraw)) {
-		ACCESS_FBINFO(cursor.redraw) = 0;
-		ACCESS_FBINFO(cursor.x) = x;
-		ACCESS_FBINFO(cursor.y) = y;
-		x += 64;
-		y += 64;
-		outDAC1064(PMINFO M1064_XCURCTRL, M1064_XCURCTRL_DIS);
-		mga_outb(M_RAMDAC_BASE+M1064_CURPOSXL, x);
-		mga_outb(M_RAMDAC_BASE+M1064_CURPOSXH, x >> 8);
-		mga_outb(M_RAMDAC_BASE+M1064_CURPOSYL, y);
-		mga_outb(M_RAMDAC_BASE+M1064_CURPOSYH, y >> 8);
-	}
-	ACCESS_FBINFO(cursor.state) = CM_DRAW;
-	if (ACCESS_FBINFO(devflags.blink))
-		mod_timer(&ACCESS_FBINFO(cursor.timer), jiffies + HZ/2);
-	outDAC1064(PMINFO M1064_XCURCTRL, M1064_XCURCTRL_XGA);
-	matroxfb_DAC_unlock_irqrestore(flags);
-}
-
-static int matroxfb_DAC1064_setfont(struct display* p, int width, int height) {
-	if (p && p->conp)
-		matroxfb_DAC1064_createcursor(PMXINFO(p) p);
-	return 0;
-}
-
-static int DAC1064_selhwcursor(WPMINFO2) {
-	ACCESS_FBINFO(dispsw.cursor) = matroxfb_DAC1064_cursor;
-	ACCESS_FBINFO(dispsw.set_font) = matroxfb_DAC1064_setfont;
-	return 0;
-}
-
 static void DAC1064_calcclock(CPMINFO unsigned int freq, unsigned int fmax, unsigned int* in, unsigned int* feed, unsigned int* post) {
 	unsigned int fvco;
 	unsigned int p;
 
-	DBG("DAC1064_calcclock")
+	DBG(__FUNCTION__)
 	
 	/* only for devices older than G450 */
 
@@ -206,7 +85,7 @@ static const unsigned char MGA1064_DAC[] = {
 static void DAC1064_setpclk(WPMINFO unsigned long fout) {
 	unsigned int m, n, p;
 
-	DBG("DAC1064_setpclk")
+	DBG(__FUNCTION__)
 
 	DAC1064_calcclock(PMINFO fout, ACCESS_FBINFO(max_pixel_clock), &m, &n, &p);
 	ACCESS_FBINFO(hw).DACclk[0] = m;
@@ -218,7 +97,7 @@ static void DAC1064_setmclk(WPMINFO int oscinfo, unsigned long fmem) {
 	u_int32_t mx;
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("DAC1064_setmclk")
+	DBG(__FUNCTION__)
 
 	if (ACCESS_FBINFO(devflags.noinit)) {
 		/* read MCLK and give up... */
@@ -461,15 +340,10 @@ void DAC1064_global_restore(WPMINFO2) {
 static int DAC1064_init_1(WPMINFO struct my_timming* m) {
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("DAC1064_init_1")
+	DBG(__FUNCTION__)
 
 	memcpy(hw->DACreg, MGA1064_DAC, sizeof(MGA1064_DAC_regs));
-	if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-		hw->DACreg[POS1064_XMISCCTRL] = M1064_XMISCCTRL_DAC_6BIT;
-		hw->DACreg[POS1064_XMULCTRL] = M1064_XMULCTRL_DEPTH_8BPP
-					     | M1064_XMULCTRL_GRAPHICS_PALETIZED;
-	} else {
-		switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
+	switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
 		/* case 4: not supported by MGA1064 DAC */
 		case 8:
 			hw->DACreg[POS1064_XMULCTRL] = M1064_XMULCTRL_DEPTH_8BPP | M1064_XMULCTRL_GRAPHICS_PALETIZED;
@@ -488,13 +362,12 @@ static int DAC1064_init_1(WPMINFO struct my_timming* m) {
 			break;
 		default:
 			return 1;	/* unsupported depth */
-		}
 	}
 	hw->DACreg[POS1064_XVREFCTRL] = ACCESS_FBINFO(features.DAC1064.xvrefctrl);
 	hw->DACreg[POS1064_XGENCTRL] &= ~M1064_XGENCTRL_SYNC_ON_GREEN_MASK;
 	hw->DACreg[POS1064_XGENCTRL] |= (m->sync & FB_SYNC_ON_GREEN)?M1064_XGENCTRL_SYNC_ON_GREEN:M1064_XGENCTRL_NO_SYNC_ON_GREEN;
-	hw->DACreg[POS1064_XCURADDL] = ACCESS_FBINFO(features.DAC1064.cursorimage) >> 10;
-	hw->DACreg[POS1064_XCURADDH] = ACCESS_FBINFO(features.DAC1064.cursorimage) >> 18;
+	hw->DACreg[POS1064_XCURADDL] = 0;
+	hw->DACreg[POS1064_XCURADDH] = 0;
 
 	DAC1064_global_init(PMINFO2);
 	return 0;
@@ -503,7 +376,7 @@ static int DAC1064_init_1(WPMINFO struct my_timming* m) {
 static int DAC1064_init_2(WPMINFO struct my_timming* m) {
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("DAC1064_init_2")
+	DBG(__FUNCTION__)
 
 	if (ACCESS_FBINFO(fbcon).var.bits_per_pixel > 16) {	/* 256 entries */
 		int i;
@@ -547,7 +420,7 @@ static void DAC1064_restore_1(WPMINFO2) {
 
 	CRITFLAGS
 
-	DBG("DAC1064_restore_1")
+	DBG(__FUNCTION__)
 
 	CRITBEGIN
 
@@ -572,14 +445,13 @@ static void DAC1064_restore_1(WPMINFO2) {
 	CRITEND
 };
 
-static void DAC1064_restore_2(WPMINFO struct display* p) {
+static void DAC1064_restore_2(WPMINFO2) {
 #ifdef DEBUG
 	unsigned int i;
 #endif
 
-	DBG("DAC1064_restore_2")
+	DBG(__FUNCTION__)
 
-	matrox_init_putc(PMINFO p, matroxfb_DAC1064_createcursor);
 #ifdef DEBUG
 	dprintk(KERN_DEBUG "DAC1064regs ");
 	for (i = 0; i < sizeof(MGA1064_DAC_regs); i++) {
@@ -648,13 +520,13 @@ static struct matrox_altout g450out = {
 #endif /* NEED_DAC1064 */
 
 #ifdef CONFIG_FB_MATROX_MYSTIQUE
-static int MGA1064_init(WPMINFO struct my_timming* m, struct display* p) {
+static int MGA1064_init(WPMINFO struct my_timming* m) {
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("MGA1064_init")
+	DBG(__FUNCTION__)
 
 	if (DAC1064_init_1(PMINFO m)) return 1;
-	if (matroxfb_vgaHWinit(PMINFO m, p)) return 1;
+	if (matroxfb_vgaHWinit(PMINFO m)) return 1;
 
 	hw->MiscOutReg = 0xCB;
 	if (m->sync & FB_SYNC_HOR_HIGH_ACT)
@@ -670,14 +542,14 @@ static int MGA1064_init(WPMINFO struct my_timming* m, struct display* p) {
 #endif
 
 #ifdef CONFIG_FB_MATROX_G100
-static int MGAG100_init(WPMINFO struct my_timming* m, struct display* p) {
+static int MGAG100_init(WPMINFO struct my_timming* m) {
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("MGAG100_init")
+	DBG(__FUNCTION__)
 
 	if (DAC1064_init_1(PMINFO m)) return 1;
 	hw->MXoptionReg &= ~0x2000;
-	if (matroxfb_vgaHWinit(PMINFO m, p)) return 1;
+	if (matroxfb_vgaHWinit(PMINFO m)) return 1;
 
 	hw->MiscOutReg = 0xEF;
 	if (m->sync & FB_SYNC_HOR_HIGH_ACT)
@@ -695,7 +567,7 @@ static int MGAG100_init(WPMINFO struct my_timming* m, struct display* p) {
 #ifdef CONFIG_FB_MATROX_MYSTIQUE
 static void MGA1064_ramdac_init(WPMINFO2) {
 
-	DBG("MGA1064_ramdac_init");
+	DBG(__FUNCTION__)
 
 	/* ACCESS_FBINFO(features.DAC1064.vco_freq_min) = 120000; */
 	ACCESS_FBINFO(features.pll.vco_freq_min) = 62000;
@@ -724,7 +596,7 @@ static void MGAG100_progPixClock(CPMINFO int flags, int m, int n, int p) {
 	int selClk;
 	int clk;
 
-	DBG("MGAG100_progPixClock")
+	DBG(__FUNCTION__)
 
 	outDAC1064(PMINFO M1064_XPIXCLKCTRL, inDAC1064(PMINFO M1064_XPIXCLKCTRL) | M1064_XPIXCLKCTRL_DIS |
 		   M1064_XPIXCLKCTRL_PLL_UP);
@@ -766,7 +638,7 @@ static void MGAG100_progPixClock(CPMINFO int flags, int m, int n, int p) {
 static void MGAG100_setPixClock(CPMINFO int flags, int freq) {
 	unsigned int m, n, p;
 
-	DBG("MGAG100_setPixClock")
+	DBG(__FUNCTION__)
 
 	DAC1064_calcclock(PMINFO freq, ACCESS_FBINFO(max_pixel_clock), &m, &n, &p);
 	MGAG100_progPixClock(PMINFO flags, m, n, p);
@@ -780,13 +652,12 @@ static int MGA1064_preinit(WPMINFO2) {
 					     2048,    0};
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("MGA1064_preinit")
+	DBG(__FUNCTION__)
 
 	/* ACCESS_FBINFO(capable.cfb4) = 0; ... preinitialized by 0 */
 	ACCESS_FBINFO(capable.text) = 1;
 	ACCESS_FBINFO(capable.vxres) = vxres_mystique;
 	ACCESS_FBINFO(features.accel.has_cacheflush) = 1;
-	ACCESS_FBINFO(cursor.timer.function) = matroxfb_DAC1064_flashcursor;
 
 	ACCESS_FBINFO(outputs[0]).output = &m1064;
 	ACCESS_FBINFO(outputs[0]).src = MATROXFB_SRC_CRTC1;
@@ -815,12 +686,8 @@ static int MGA1064_preinit(WPMINFO2) {
 
 static void MGA1064_reset(WPMINFO2) {
 
-	DBG("MGA1064_reset");
+	DBG(__FUNCTION__);
 
-	ACCESS_FBINFO(features.DAC1064.cursorimage) = ACCESS_FBINFO(video.len_usable) - 1024;
-	if (ACCESS_FBINFO(devflags.hwcursor))
-		ACCESS_FBINFO(video.len_usable) -= 1024;
-	matroxfb_fastfont_init(MINFO);
 	MGA1064_ramdac_init(PMINFO2);
 }
 #endif
@@ -960,7 +827,7 @@ static int MGAG100_preinit(WPMINFO2) {
 	u_int32_t q;
 #endif
 
-	DBG("MGAG100_preinit")
+	DBG(__FUNCTION__)
 
 	/* there are some instabilities if in_div > 19 && vco < 61000 */
 	if (ACCESS_FBINFO(devflags.g450dac)) {
@@ -981,7 +848,6 @@ static int MGAG100_preinit(WPMINFO2) {
 	ACCESS_FBINFO(capable.text) = 1;
 	ACCESS_FBINFO(capable.vxres) = vxres_g100;
 	ACCESS_FBINFO(features.accel.has_cacheflush) = 1;
-	ACCESS_FBINFO(cursor.timer.function) = matroxfb_DAC1064_flashcursor;
 	ACCESS_FBINFO(capable.plnwt) = ACCESS_FBINFO(devflags.accelerator) == FB_ACCEL_MATROX_MGAG100
 			? ACCESS_FBINFO(devflags.sgram) : 1;
 
@@ -1099,12 +965,7 @@ static void MGAG100_reset(WPMINFO2) {
 	u_int8_t b;
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("MGAG100_reset")
-
-	ACCESS_FBINFO(features.DAC1064.cursorimage) = ACCESS_FBINFO(video.len_usable) - 1024;
-	if (ACCESS_FBINFO(devflags.hwcursor))
-		ACCESS_FBINFO(video.len_usable) -= 1024;
-	matroxfb_fastfont_init(MINFO);
+	DBG(__FUNCTION__)
 
 	{
 #ifdef G100_BROKEN_IBM_82351
@@ -1157,13 +1018,13 @@ static void MGAG100_reset(WPMINFO2) {
 #endif
 
 #ifdef CONFIG_FB_MATROX_MYSTIQUE
-static void MGA1064_restore(WPMINFO struct display* p) {
+static void MGA1064_restore(WPMINFO2) {
 	int i;
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
 	CRITFLAGS
 
-	DBG("MGA1064_restore")
+	DBG(__FUNCTION__)
 
 	CRITBEGIN
 
@@ -1177,18 +1038,18 @@ static void MGA1064_restore(WPMINFO struct display* p) {
 	matroxfb_vgaHWrestore(PMINFO2);
 	for (i = 0; i < 6; i++)
 		mga_setr(M_EXTVGA_INDEX, i, hw->CRTCEXT[i]);
-	DAC1064_restore_2(PMINFO p);
+	DAC1064_restore_2(PMINFO2);
 }
 #endif
 
 #ifdef CONFIG_FB_MATROX_G100
-static void MGAG100_restore(WPMINFO struct display* p) {
+static void MGAG100_restore(WPMINFO2) {
 	int i;
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
 	CRITFLAGS
 
-	DBG("MGAG100_restore")
+	DBG(__FUNCTION__)
 
 	CRITBEGIN
 
@@ -1203,20 +1064,20 @@ static void MGAG100_restore(WPMINFO struct display* p) {
 #endif
 	for (i = 0; i < 6; i++)
 		mga_setr(M_EXTVGA_INDEX, i, hw->CRTCEXT[i]);
-	DAC1064_restore_2(PMINFO p);
+	DAC1064_restore_2(PMINFO2);
 }
 #endif
 
 #ifdef CONFIG_FB_MATROX_MYSTIQUE
 struct matrox_switch matrox_mystique = {
-	MGA1064_preinit, MGA1064_reset, MGA1064_init, MGA1064_restore, DAC1064_selhwcursor
+	MGA1064_preinit, MGA1064_reset, MGA1064_init, MGA1064_restore,
 };
 EXPORT_SYMBOL(matrox_mystique);
 #endif
 
 #ifdef CONFIG_FB_MATROX_G100
 struct matrox_switch matrox_G100 = {
-	MGAG100_preinit, MGAG100_reset, MGAG100_init, MGAG100_restore, DAC1064_selhwcursor
+	MGAG100_preinit, MGAG100_reset, MGAG100_init, MGAG100_restore,
 };
 EXPORT_SYMBOL(matrox_G100);
 #endif

drivers/video/matrox/matroxfb_Ti3026.c

@@ -281,138 +281,11 @@ static const unsigned char MGADACbpp32[] =
   TVP3026_XCOLKEYCTRL_ZOOM1,
   0x00, 0x00, TVP3026_XCURCTRL_DIS };
 
-static void matroxfb_ti3026_flashcursor(unsigned long ptr) {
-	unsigned long flags;
-
-#define minfo ((struct matrox_fb_info*)ptr)
-	matroxfb_DAC_lock_irqsave(flags);
-	outTi3026(PMINFO TVP3026_XCURCTRL, inTi3026(PMINFO TVP3026_XCURCTRL) ^ TVP3026_XCURCTRL_DIS ^ TVP3026_XCURCTRL_XGA);
-	ACCESS_FBINFO(cursor.timer.expires) = jiffies + HZ/2;
-	add_timer(&ACCESS_FBINFO(cursor.timer));
-	matroxfb_DAC_unlock_irqrestore(flags);
-#undef minfo
-}
-
-static void matroxfb_ti3026_createcursor(WPMINFO struct display* p) {
-	unsigned long flags;
-	u_int32_t xline;
-	unsigned int i;
-	unsigned int to;
-
-	if (ACCESS_FBINFO(currcon_display) != p)
-		return;
-
-	DBG("matroxfb_ti3026_createcursor");
-
-	matroxfb_createcursorshape(PMINFO p, p->var.vmode);
-
-	xline = (~0) << (32 - ACCESS_FBINFO(cursor.w));
-	matroxfb_DAC_lock_irqsave(flags);
-	mga_outb(M_RAMDAC_BASE+TVP3026_INDEX, 0);
-	to = ACCESS_FBINFO(cursor.u);
-	for (i = 0; i < to; i++) {
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-	}
-	to = ACCESS_FBINFO(cursor.d);
-	for (; i < to; i++) {
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, xline >> 24);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, xline >> 16);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, xline >> 8);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, xline);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-	}
-	for (; i < 64; i++) {
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0);
-	}
-	for (i = 0; i < 512; i++)
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURRAMDATA, 0xFF);
-	matroxfb_DAC_unlock_irqrestore(flags);
-}
-
-static void matroxfb_ti3026_cursor(struct display* p, int mode, int x, int y) {
-	unsigned long flags;
-	MINFO_FROM_DISP(p);
-
-	DBG("matroxfb_ti3026_cursor")
-
-	if (ACCESS_FBINFO(currcon_display) != p)
-		return;
-
-	if (mode == CM_ERASE) {
-		if (ACCESS_FBINFO(cursor.state) != CM_ERASE) {
-			del_timer_sync(&ACCESS_FBINFO(cursor.timer));
-			matroxfb_DAC_lock_irqsave(flags);
-			ACCESS_FBINFO(cursor.state) = CM_ERASE;
-			outTi3026(PMINFO TVP3026_XCURCTRL, ACCESS_FBINFO(hw.DACreg[POS3026_XCURCTRL]));
-			matroxfb_DAC_unlock_irqrestore(flags);
-		}
-		return;
-	}
-	if ((p->conp->vc_cursor_type & CUR_HWMASK) != ACCESS_FBINFO(cursor.type))
-		matroxfb_ti3026_createcursor(PMINFO p);
-	x *= fontwidth(p);
-	y *= fontheight(p);
-	y -= p->var.yoffset;
-	if (p->var.vmode & FB_VMODE_DOUBLE)
-		y *= 2;
-	del_timer_sync(&ACCESS_FBINFO(cursor.timer));
-	matroxfb_DAC_lock_irqsave(flags);
-	if ((x != ACCESS_FBINFO(cursor.x)) || (y != ACCESS_FBINFO(cursor.y)) || ACCESS_FBINFO(cursor.redraw)) {
-		ACCESS_FBINFO(cursor.redraw) = 0;
-		ACCESS_FBINFO(cursor.x) = x;
-		ACCESS_FBINFO(cursor.y) = y;
-		x += 64;
-		y += 64;
-		outTi3026(PMINFO TVP3026_XCURCTRL, ACCESS_FBINFO(hw.DACreg[POS3026_XCURCTRL]));
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURPOSXL, x);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURPOSXH, x >> 8);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURPOSYL, y);
-		mga_outb(M_RAMDAC_BASE+TVP3026_CURPOSYH, y >> 8);
-	}
-	ACCESS_FBINFO(cursor.state) = CM_DRAW;
-	if (ACCESS_FBINFO(devflags.blink))
-		mod_timer(&ACCESS_FBINFO(cursor.timer), jiffies + HZ/2);
-	outTi3026(PMINFO TVP3026_XCURCTRL, ACCESS_FBINFO(hw.DACreg[POS3026_XCURCTRL]) | TVP3026_XCURCTRL_XGA);
-	matroxfb_DAC_unlock_irqrestore(flags);
-}
-
-static int matroxfb_ti3026_setfont(struct display* p, int width, int height) {
-
-	DBG("matrox_ti3026_setfont");
-
-	if (p && p->conp)
-		matroxfb_ti3026_createcursor(PMXINFO(p) p);
-	return 0;
-}
-
-static int matroxfb_ti3026_selhwcursor(WPMINFO2) {
-	ACCESS_FBINFO(dispsw.cursor) = matroxfb_ti3026_cursor;
-	ACCESS_FBINFO(dispsw.set_font) = matroxfb_ti3026_setfont;
-	return 0;
-}
-
 static int Ti3026_calcclock(CPMINFO unsigned int freq, unsigned int fmax, int* in, int* feed, int* post) {
 	unsigned int fvco;
 	unsigned int lin, lfeed, lpost;
 
-	DBG("Ti3026_calcclock")
+	DBG(__FUNCTION__)
 
 	fvco = PLL_calcclock(PMINFO freq, fmax, &lin, &lfeed, &lpost);
 	fvco >>= (*post = lpost);
@@ -421,12 +294,12 @@ static int Ti3026_calcclock(CPMINFO unsigned int freq, unsigned int fmax, int* i
 	return fvco;
 }
 
-static int Ti3026_setpclk(WPMINFO int clk, struct display* p) {
+static int Ti3026_setpclk(WPMINFO int clk) {
 	unsigned int f_pll;
 	unsigned int pixfeed, pixin, pixpost;
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("Ti3026_setpclk")
+	DBG(__FUNCTION__)
 
 	f_pll = Ti3026_calcclock(PMINFO clk, ACCESS_FBINFO(max_pixel_clock), &pixin, &pixfeed, &pixpost);
 
@@ -434,18 +307,13 @@ static int Ti3026_setpclk(WPMINFO int clk, struct display* p) {
 	hw->DACclk[1] = pixfeed;
 	hw->DACclk[2] = pixpost | 0xB0;
 
-	if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-		hw->DACreg[POS3026_XMEMPLLCTRL] = TVP3026_XMEMPLLCTRL_MCLK_MCLKPLL | TVP3026_XMEMPLLCTRL_RCLK_PIXPLL;
-		hw->DACclk[3] = 0xFD;
-		hw->DACclk[4] = 0x3D;
-		hw->DACclk[5] = 0x70;
-	} else {
+	{
 		unsigned int loopfeed, loopin, looppost, loopdiv, z;
 		unsigned int Bpp;
 
 		Bpp = ACCESS_FBINFO(curr.final_bppShift);
 
-		if (p->var.bits_per_pixel == 24) {
+		if (ACCESS_FBINFO(fbcon).var.bits_per_pixel == 24) {
 			loopfeed = 3;		/* set lm to any possible value */
 			loopin = 3 * 32 / Bpp;
 		} else {
@@ -464,7 +332,7 @@ static int Ti3026_setpclk(WPMINFO int clk, struct display* p) {
 			looppost = 3;
 			loopdiv = z/16;
 		}
-		if (p->var.bits_per_pixel == 24) {
+		if (ACCESS_FBINFO(fbcon).var.bits_per_pixel == 24) {
 			hw->DACclk[3] = ((65 - loopin) & 0x3F) | 0xC0;
 			hw->DACclk[4] = (65 - loopfeed) | 0x80;
 			if (ACCESS_FBINFO(accel.ramdac_rev) > 0x20) {
@@ -495,22 +363,14 @@ static int Ti3026_setpclk(WPMINFO int clk, struct display* p) {
 	return 0;
 }
 
-static int Ti3026_init(WPMINFO struct my_timming* m, struct display* p) {
+static int Ti3026_init(WPMINFO struct my_timming* m) {
 	u_int8_t muxctrl = isInterleave(MINFO) ? TVP3026_XMUXCTRL_MEMORY_64BIT : TVP3026_XMUXCTRL_MEMORY_32BIT;
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("Ti3026_init")
+	DBG(__FUNCTION__)
 
 	memcpy(hw->DACreg, MGADACbpp32, sizeof(hw->DACreg));
-	if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-		hw->DACreg[POS3026_XLATCHCTRL] = TVP3026_XLATCHCTRL_8_1;
-		hw->DACreg[POS3026_XTRUECOLORCTRL] = TVP3026_XTRUECOLORCTRL_PSEUDOCOLOR;
-		hw->DACreg[POS3026_XMUXCTRL] = TVP3026_XMUXCTRL_VGA;
-		hw->DACreg[POS3026_XCLKCTRL] = TVP3026_XCLKCTRL_SRC_PLL |
-					       TVP3026_XCLKCTRL_DIV4;
-		hw->DACreg[POS3026_XMISCCTRL] = TVP3026_XMISCCTRL_DAC_PUP | TVP3026_XMISCCTRL_DAC_6BIT | TVP3026_XMISCCTRL_PSEL_DIS | TVP3026_XMISCCTRL_PSEL_LOW;
-	} else {
-		switch (p->var.bits_per_pixel) {
+	switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
 		case 4:	hw->DACreg[POS3026_XLATCHCTRL] = TVP3026_XLATCHCTRL_16_1;	/* or _8_1, they are same */
 			hw->DACreg[POS3026_XTRUECOLORCTRL] = TVP3026_XTRUECOLORCTRL_PSEUDOCOLOR;
 			hw->DACreg[POS3026_XMUXCTRL] = muxctrl | TVP3026_XMUXCTRL_PIXEL_4BIT;
@@ -525,7 +385,7 @@ static int Ti3026_init(WPMINFO struct my_timming* m, struct display* p) {
 			break;
 		case 16:
 			/* XLATCHCTRL should be _4_1 / _2_1... Why is not? (_2_1 is used everytime) */
-			hw->DACreg[POS3026_XTRUECOLORCTRL] = (p->var.green.length == 5)? (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_ORGB_1555 ) : (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_RGB_565);
+			hw->DACreg[POS3026_XTRUECOLORCTRL] = (ACCESS_FBINFO(fbcon).var.green.length == 5)? (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_ORGB_1555 ) : (TVP3026_XTRUECOLORCTRL_DIRECTCOLOR | TVP3026_XTRUECOLORCTRL_RGB_565);
 			hw->DACreg[POS3026_XMUXCTRL] = muxctrl | TVP3026_XMUXCTRL_PIXEL_16BIT;
 			hw->DACreg[POS3026_XCLKCTRL] = TVP3026_XCLKCTRL_SRC_PLL | TVP3026_XCLKCTRL_DIV2;
 			break;
@@ -541,9 +401,8 @@ static int Ti3026_init(WPMINFO struct my_timming* m, struct display* p) {
 			break;
 		default:
 			return 1;	/* TODO: failed */
-		}
 	}
-	if (matroxfb_vgaHWinit(PMINFO m, p)) return 1;
+	if (matroxfb_vgaHWinit(PMINFO m)) return 1;
 
 	/* set SYNC */
 	hw->MiscOutReg = 0xCB;
@@ -569,10 +428,10 @@ static int Ti3026_init(WPMINFO struct my_timming* m, struct display* p) {
 
 	/* set interleaving */
 	hw->MXoptionReg &= ~0x00001000;
-	if ((ACCESS_FBINFO(fbcon).fix.type != FB_TYPE_TEXT) && isInterleave(MINFO)) hw->MXoptionReg |= 0x00001000;
+	if (isInterleave(MINFO)) hw->MXoptionReg |= 0x00001000;
 
 	/* set DAC */
-	Ti3026_setpclk(PMINFO m->pixclock, p);
+	Ti3026_setpclk(PMINFO m->pixclock);
 	return 0;
 }
 
@@ -583,7 +442,7 @@ static void ti3026_setMCLK(WPMINFO int fout){
 	unsigned int rfhcnt, mclk_ctl;
 	int tmout;
 
-	DBG("ti3026_setMCLK")
+	DBG(__FUNCTION__)
 
 	f_pll = Ti3026_calcclock(PMINFO fout, ACCESS_FBINFO(max_pixel_clock), &mclk_n, &mclk_m, &mclk_p);
 
@@ -677,7 +536,7 @@ static void ti3026_setMCLK(WPMINFO int fout){
 
 static void ti3026_ramdac_init(WPMINFO2) {
 
-	DBG("ti3026_ramdac_init")
+	DBG(__FUNCTION__)
 
 	ACCESS_FBINFO(features.pll.vco_freq_min) = 110000;
 	ACCESS_FBINFO(features.pll.ref_freq)	 = 114545;
@@ -691,13 +550,13 @@ static void ti3026_ramdac_init(WPMINFO2) {
 	ti3026_setMCLK(PMINFO 60000);
 }
 
-static void Ti3026_restore(WPMINFO struct display* p) {
+static void Ti3026_restore(WPMINFO2) {
 	int i;
 	unsigned char progdac[6];
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 	CRITFLAGS
 
-	DBG("Ti3026_restore")
+	DBG(__FUNCTION__)
 
 #ifdef DEBUG
 	dprintk(KERN_INFO "EXTVGA regs: ");
@@ -788,7 +647,6 @@ static void Ti3026_restore(WPMINFO struct display* p) {
 				dprintk(KERN_INFO "LoopPLL: %d\n", 500000-tmout);
 		}
 	}
-	matrox_init_putc(PMINFO p, matroxfb_ti3026_createcursor);
 
 #ifdef DEBUG
 	dprintk(KERN_DEBUG "3026DACregs ");
@@ -805,9 +663,7 @@ static void Ti3026_restore(WPMINFO struct display* p) {
 
 static void Ti3026_reset(WPMINFO2) {
 
-	DBG("Ti3026_reset")
-
-	matroxfb_fastfont_init(MINFO);
+	DBG(__FUNCTION__)
 
 	ti3026_ramdac_init(PMINFO2);
 }
@@ -825,14 +681,13 @@ static int Ti3026_preinit(WPMINFO2) {
 					  2048, 0};
 	struct matrox_hw_state* hw = &ACCESS_FBINFO(hw);
 
-	DBG("Ti3026_preinit")
+	DBG(__FUNCTION__)
 
 	ACCESS_FBINFO(millenium) = 1;
 	ACCESS_FBINFO(milleniumII) = (ACCESS_FBINFO(pcidev)->device != PCI_DEVICE_ID_MATROX_MIL);
 	ACCESS_FBINFO(capable.cfb4) = 1;
 	ACCESS_FBINFO(capable.text) = 1; /* isMilleniumII(MINFO); */
 	ACCESS_FBINFO(capable.vxres) = isMilleniumII(MINFO)?vxres_mill2:vxres_mill1;
-	ACCESS_FBINFO(cursor.timer.function) = matroxfb_ti3026_flashcursor;
 
 	ACCESS_FBINFO(outputs[0]).data = MINFO;
 	ACCESS_FBINFO(outputs[0]).output = &ti3026_output;
@@ -876,7 +731,7 @@ static int Ti3026_preinit(WPMINFO2) {
 }
 
 struct matrox_switch matrox_millennium = {
-	Ti3026_preinit, Ti3026_reset, Ti3026_init, Ti3026_restore, matroxfb_ti3026_selhwcursor
+	Ti3026_preinit, Ti3026_reset, Ti3026_init, Ti3026_restore
 };
 EXPORT_SYMBOL(matrox_millennium);
 #endif

drivers/video/matrox/matroxfb_accel.c

@@ -85,44 +85,101 @@
 
 #define mga_ydstlen(y,l) mga_outl(M_YDSTLEN | M_EXEC, ((y) << 16) | (l))
 
+static inline void matrox_cfb4_pal(u_int32_t* pal) {
+	unsigned int i;
+	
+	for (i = 0; i < 16; i++) {
+		pal[i] = i * 0x11111111U;
+	}
+	pal[i] = 0xFFFFFFFF;
+}
+
+static inline void matrox_cfb8_pal(u_int32_t* pal) {
+	unsigned int i;
+	
+	for (i = 0; i < 16; i++) {
+		pal[i] = i * 0x01010101U;
+	}
+	pal[i] = 0x0F0F0F0F;
+}
+
+static void matroxfb_copyarea(struct fb_info* info, const struct fb_copyarea* area);
+static void matroxfb_fillrect(struct fb_info* info, const struct fb_fillrect* rect);
+static void matroxfb_imageblit(struct fb_info* info, const struct fb_image* image);
+static void matroxfb_cfb4_fillrect(struct fb_info* info, const struct fb_fillrect* rect);
+static void matroxfb_cfb4_copyarea(struct fb_info* info, const struct fb_copyarea* area);
+
 void matrox_cfbX_init(WPMINFO2) {
 	u_int32_t maccess;
 	u_int32_t mpitch;
 	u_int32_t mopmode;
+	int accel;
 
-	DBG("matrox_cfbX_init")
+	DBG(__FUNCTION__)
 
 	mpitch = ACCESS_FBINFO(fbcon).var.xres_virtual;
 
-	if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-		maccess = 0x00000000;
-		mpitch = (mpitch >> 4) | 0x8000; /* set something */
-		mopmode = M_OPMODE_8BPP;
-	} else {
-		switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
+	ACCESS_FBINFO(fbops).fb_copyarea = cfb_copyarea;
+	ACCESS_FBINFO(fbops).fb_fillrect = cfb_fillrect;
+	ACCESS_FBINFO(fbops).fb_imageblit = cfb_imageblit;
+	ACCESS_FBINFO(fbops).fb_cursor = soft_cursor;
+
+	accel = (ACCESS_FBINFO(fbcon).var.accel_flags & FB_ACCELF_TEXT) == FB_ACCELF_TEXT;
+
+	switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
 		case 4:		maccess = 0x00000000;	/* accelerate as 8bpp video */
 				mpitch = (mpitch >> 1) | 0x8000; /* disable linearization */
 				mopmode = M_OPMODE_4BPP;
+				matrox_cfb4_pal(ACCESS_FBINFO(cmap));
+				if (accel && !(mpitch & 1)) {
+					ACCESS_FBINFO(fbops).fb_copyarea = matroxfb_cfb4_copyarea;
+					ACCESS_FBINFO(fbops).fb_fillrect = matroxfb_cfb4_fillrect;
+				}
 				break;
 		case 8:		maccess = 0x00000000;
 				mopmode = M_OPMODE_8BPP;
+				matrox_cfb8_pal(ACCESS_FBINFO(cmap));
+				if (accel) {
+					ACCESS_FBINFO(fbops).fb_copyarea = matroxfb_copyarea;
+					ACCESS_FBINFO(fbops).fb_fillrect = matroxfb_fillrect;
+					ACCESS_FBINFO(fbops).fb_imageblit = matroxfb_imageblit;
+				}
 				break;
-		case 16:	if (ACCESS_FBINFO(fbcon).var.green.length == 5)
+		case 16:	if (ACCESS_FBINFO(fbcon).var.green.length == 5) {
 					maccess = 0xC0000001;
-				else
+					ACCESS_FBINFO(cmap[16]) = 0x7FFF7FFF;
+				} else {
 					maccess = 0x40000001;
+					ACCESS_FBINFO(cmap[16]) = 0xFFFFFFFF;
+				}
 				mopmode = M_OPMODE_16BPP;
+				if (accel) {
+					ACCESS_FBINFO(fbops).fb_copyarea = matroxfb_copyarea;
+					ACCESS_FBINFO(fbops).fb_fillrect = matroxfb_fillrect;
+					ACCESS_FBINFO(fbops).fb_imageblit = matroxfb_imageblit;
+				}
 				break;
 		case 24:	maccess = 0x00000003;
 				mopmode = M_OPMODE_24BPP;
+				ACCESS_FBINFO(cmap[16]) = 0xFFFFFFFF;
+				if (accel) {
+					ACCESS_FBINFO(fbops).fb_copyarea = matroxfb_copyarea;
+					ACCESS_FBINFO(fbops).fb_fillrect = matroxfb_fillrect;
+					ACCESS_FBINFO(fbops).fb_imageblit = matroxfb_imageblit;
+				}
 				break;
 		case 32:	maccess = 0x00000002;
 				mopmode = M_OPMODE_32BPP;
+				ACCESS_FBINFO(cmap[16]) = 0xFFFFFFFF;
+				if (accel) {
+					ACCESS_FBINFO(fbops).fb_copyarea = matroxfb_copyarea;
+					ACCESS_FBINFO(fbops).fb_fillrect = matroxfb_fillrect;
+					ACCESS_FBINFO(fbops).fb_imageblit = matroxfb_imageblit;
+				}
 				break;
 		default:	maccess = 0x00000000;
 				mopmode = 0x00000000;
 				break;	/* turn off acceleration!!! */
-		}
 	}
 	mga_fifo(8);
 	mga_outl(M_PITCH, mpitch);
@@ -145,36 +202,29 @@ void matrox_cfbX_init(WPMINFO2) {
 
 EXPORT_SYMBOL(matrox_cfbX_init);
 
-static void matrox_cfbX_bmove(struct display* p, int sy, int sx, int dy, int dx, int height, int width) {
-	int pixx = p->var.xres_virtual, start, end;
+static void matrox_accel_bmove(WPMINFO int vxres, int sy, int sx, int dy, int dx, int height, int width) {
+	int start, end;
 	CRITFLAGS
-	MINFO_FROM_DISP(p);
 
-	DBG("matrox_cfbX_bmove")
+	DBG(__FUNCTION__)
 
 	CRITBEGIN
 
-	sx *= fontwidth(p);
-	dx *= fontwidth(p);
-	width *= fontwidth(p);
-	height *= fontheight(p);
-	sy *= fontheight(p);
-	dy *= fontheight(p);
 	if ((dy < sy) || ((dy == sy) && (dx <= sx))) {
 		mga_fifo(2);
 		mga_outl(M_DWGCTL, M_DWG_BITBLT | M_DWG_SHIFTZERO | M_DWG_SGNZERO |
 			 M_DWG_BFCOL | M_DWG_REPLACE);
-		mga_outl(M_AR5, pixx);
+		mga_outl(M_AR5, vxres);
 		width--;
-		start = sy*pixx+sx+curr_ydstorg(MINFO);
+		start = sy*vxres+sx+curr_ydstorg(MINFO);
 		end = start+width;
 	} else {
 		mga_fifo(3);
 		mga_outl(M_DWGCTL, M_DWG_BITBLT | M_DWG_SHIFTZERO | M_DWG_BFCOL | M_DWG_REPLACE);
 		mga_outl(M_SGN, 5);
-		mga_outl(M_AR5, -pixx);
+		mga_outl(M_AR5, -vxres);
 		width--;
-		end = (sy+height-1)*pixx+sx+curr_ydstorg(MINFO);
+		end = (sy+height-1)*vxres+sx+curr_ydstorg(MINFO);
 		start = end+width;
 		dy += height-1;
 	}
@@ -188,47 +238,29 @@ static void matrox_cfbX_bmove(struct display* p, int sy, int sx, int dy, int dx,
 	CRITEND
 }
 
-#ifdef FBCON_HAS_CFB4
-static void matrox_cfb4_bmove(struct display* p, int sy, int sx, int dy, int dx, int height, int width) {
-	int pixx, start, end;
+static void matrox_accel_bmove_lin(WPMINFO int vxres, int sy, int sx, int dy, int dx, int height, int width) {
+	int start, end;
 	CRITFLAGS
-	MINFO_FROM_DISP(p);
-	/* both (sx or dx or width) and fontwidth() are odd, so their multiply is
-	   also odd, that means that we cannot use acceleration */
 
-	DBG("matrox_cfb4_bmove")
+	DBG(__FUNCTION__)
 
 	CRITBEGIN
 
-	if ((sx | dx | width) & fontwidth(p) & 1) {
-		fbcon_cfb4_bmove(p, sy, sx, dy, dx, height, width);
-		return;
-	}
-	sx *= fontwidth(p);
-	dx *= fontwidth(p);
-	width *= fontwidth(p);
-	height *= fontheight(p);
-	sy *= fontheight(p);
-	dy *= fontheight(p);
-	pixx = p->var.xres_virtual >> 1;
-	sx >>= 1;
-	dx >>= 1;
-	width >>= 1;
 	if ((dy < sy) || ((dy == sy) && (dx <= sx))) {
 		mga_fifo(2);
-		mga_outl(M_AR5, pixx);
 		mga_outl(M_DWGCTL, M_DWG_BITBLT | M_DWG_SHIFTZERO | M_DWG_SGNZERO |
 			M_DWG_BFCOL | M_DWG_REPLACE);
+		mga_outl(M_AR5, vxres);
 		width--;
-		start = sy*pixx+sx+curr_ydstorg(MINFO);
+		start = sy*vxres+sx+curr_ydstorg(MINFO);
 		end = start+width;
 	} else {
 		mga_fifo(3);
-		mga_outl(M_SGN, 5);
-		mga_outl(M_AR5, -pixx);
 		mga_outl(M_DWGCTL, M_DWG_BITBLT | M_DWG_SHIFTZERO | M_DWG_BFCOL | M_DWG_REPLACE);
+		mga_outl(M_SGN, 5);
+		mga_outl(M_AR5, -vxres);
 		width--;
-		end = (sy+height-1)*pixx+sx+curr_ydstorg(MINFO);
+		end = (sy+height-1)*vxres+sx+curr_ydstorg(MINFO);
 		start = end+width;
 		dy += height-1;
 	}
@@ -236,19 +268,33 @@ static void matrox_cfb4_bmove(struct display* p, int sy, int sx, int dy, int dx,
 	mga_outl(M_AR0, end);
 	mga_outl(M_AR3, start);
 	mga_outl(M_FXBNDRY, ((dx+width)<<16) | dx);
-	mga_outl(M_YDST, dy*pixx >> 5);
+	mga_outl(M_YDST, dy*vxres >> 5);
 	mga_outl(M_LEN | M_EXEC, height);
 	WaitTillIdle();
 
 	CRITEND
 }
-#endif
+
+static void matroxfb_cfb4_copyarea(struct fb_info* info, const struct fb_copyarea* area) {
+	MINFO_FROM_INFO(info);
+
+	if ((area->sx | area->dx | area->width) & 1)
+		cfb_copyarea(info, area);
+	else
+		matrox_accel_bmove_lin(PMINFO ACCESS_FBINFO(fbcon.var.xres_virtual) >> 1, area->sy, area->sx >> 1, area->dy, area->dx >> 1, area->height, area->width >> 1);
+}
+
+static void matroxfb_copyarea(struct fb_info* info, const struct fb_copyarea* area) {
+	MINFO_FROM_INFO(info);
+
+	matrox_accel_bmove(PMINFO ACCESS_FBINFO(fbcon.var.xres_virtual), area->sy, area->sx, area->dy, area->dx, area->height, area->width);
+}
 
 static void matroxfb_accel_clear(WPMINFO u_int32_t color, int sy, int sx, int height,
 		int width) {
 	CRITFLAGS
 
-	DBG("matroxfb_accel_clear")
+	DBG(__FUNCTION__)
 
 	CRITBEGIN
 
@@ -262,34 +308,25 @@ static void matroxfb_accel_clear(WPMINFO u_int32_t color, int sy, int sx, int he
 	CRITEND
 }
 
-static void matrox_cfbX_clear(u_int32_t color, struct display* p, int sy, int sx, int height, int width) {
+static void matroxfb_fillrect(struct fb_info* info, const struct fb_fillrect* rect) {
+	MINFO_FROM_INFO(info);
 
-	DBG("matrox_cfbX_clear")
-
-	matroxfb_accel_clear(PMXINFO(p) color, sy * fontheight(p), sx * fontwidth(p),
-			     height * fontheight(p), width * fontwidth(p));
+	switch (rect->rop) {
+		case ROP_COPY:
+			matroxfb_accel_clear(PMINFO ((u_int32_t*)info->pseudo_palette)[rect->color], rect->dy, rect->dx, rect->height, rect->width);
+			break;
+	}
 }
 
-#ifdef FBCON_HAS_CFB4
-static void matrox_cfb4_clear(struct vc_data* conp, struct display* p, int sy, int sx, int height, int width) {
-	u_int32_t bgx;
+static void matroxfb_cfb4_clear(WPMINFO u_int32_t bgx, int sy, int sx, int height, int width) {
 	int whattodo;
 	CRITFLAGS
-	MINFO_FROM_DISP(p);
 
-	DBG("matrox_cfb4_clear")
+	DBG(__FUNCTION__)
 
 	CRITBEGIN
 
 	whattodo = 0;
-	bgx = attr_bgcol_ec(p, conp);
-	bgx |= bgx << 4;
-	bgx |= bgx << 8;
-	bgx |= bgx << 16;
-	sy *= fontheight(p);
-	sx *= fontwidth(p);
-	height *= fontheight(p);
-	width *= fontwidth(p);
 	if (sx & 1) {
 		sx ++;
 		if (!width) return;
@@ -306,12 +343,12 @@ static void matrox_cfb4_clear(struct vc_data* conp, struct display* p, int sy, i
 		mga_outl(M_DWGCTL, ACCESS_FBINFO(accel.m_dwg_rect) | M_DWG_REPLACE2);
 		mga_outl(M_FCOL, bgx);
 		mga_outl(M_FXBNDRY, ((sx + width) << 16) | sx);
-		mga_outl(M_YDST, sy * p->var.xres_virtual >> 6);
+		mga_outl(M_YDST, sy * ACCESS_FBINFO(fbcon).var.xres_virtual >> 6);
 		mga_outl(M_LEN | M_EXEC, height);
 		WaitTillIdle();
 	}
 	if (whattodo) {
-		u_int32_t step = p->var.xres_virtual >> 1;
+		u_int32_t step = ACCESS_FBINFO(fbcon).var.xres_virtual >> 1;
 		vaddr_t vbase = ACCESS_FBINFO(video.vbase);
 		if (whattodo & 1) {
 			unsigned int uaddr = sy * step + sx - 1;
@@ -335,227 +372,19 @@ static void matrox_cfb4_clear(struct vc_data* conp, struct display* p, int sy, i
 
 	CRITEND
 }
-#endif
-
-#ifdef FBCON_HAS_CFB8
-static void matrox_cfb8_clear(struct vc_data* conp, struct display* p, int sy, int sx, int height, int width) {
-	u_int32_t bgx;
-
-	DBG("matrox_cfb8_clear")
-
-	bgx = attr_bgcol_ec(p, conp);
-	bgx |= bgx << 8;
-	bgx |= bgx << 16;
-	matrox_cfbX_clear(bgx, p, sy, sx, height, width);
-}
-#endif
-
-#ifdef FBCON_HAS_CFB16
-static void matrox_cfb16_clear(struct vc_data* conp, struct display* p, int sy, int sx, int height, int width) {
-	u_int32_t bgx;
-
-	DBG("matrox_cfb16_clear")
-
-	bgx = ((u_int16_t*)p->dispsw_data)[attr_bgcol_ec(p, conp)];
-	matrox_cfbX_clear((bgx << 16) | bgx, p, sy, sx, height, width);
-}
-#endif
-
-#if defined(FBCON_HAS_CFB32) || defined(FBCON_HAS_CFB24)
-static void matrox_cfb32_clear(struct vc_data* conp, struct display* p, int sy, int sx, int height, int width) {
-	u_int32_t bgx;
-
-	DBG("matrox_cfb32_clear")
-
-	bgx = ((u_int32_t*)p->dispsw_data)[attr_bgcol_ec(p, conp)];
-	matrox_cfbX_clear(bgx, p, sy, sx, height, width);
-}
-#endif
-
-static void matrox_cfbX_fastputc(u_int32_t fgx, u_int32_t bgx, struct display* p, int c, int yy, int xx) {
-	unsigned int charcell;
-	unsigned int ar3;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	charcell = fontwidth(p) * fontheight(p);
-	yy *= fontheight(p);
-	xx *= fontwidth(p);
-
-	CRITBEGIN
-
-	mga_fifo(8);
-	mga_outl(M_DWGCTL, M_DWG_BITBLT | M_DWG_SGNZERO | M_DWG_SHIFTZERO | M_DWG_BMONOWF | M_DWG_LINEAR | M_DWG_REPLACE);
-
-	mga_outl(M_FCOL, fgx);
-	mga_outl(M_BCOL, bgx);
-	mga_outl(M_FXBNDRY, ((xx + fontwidth(p) - 1) << 16) | xx);
-	ar3 = ACCESS_FBINFO(fastfont.mgabase) + (c & p->charmask) * charcell;
-	mga_outl(M_AR3, ar3);
-	mga_outl(M_AR0, (ar3 + charcell - 1) & 0x0003FFFF);
-	mga_ydstlen(yy, fontheight(p));
-	WaitTillIdle();
-
-	CRITEND
-}
-
-static void matrox_cfbX_putc(u_int32_t fgx, u_int32_t bgx, struct display* p, int c, int yy, int xx) {
-	u_int32_t ar0;
-	u_int32_t step;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	DBG_HEAVY("matrox_cfbX_putc");
 
-	yy *= fontheight(p);
-	xx *= fontwidth(p);
+static void matroxfb_cfb4_fillrect(struct fb_info* info, const struct fb_fillrect* rect) {
+	MINFO_FROM_INFO(info);
 
-	CRITBEGIN
-
-#ifdef __BIG_ENDIAN
-	WaitTillIdle();
-	mga_outl(M_OPMODE, M_OPMODE_8BPP);
-#else
-	mga_fifo(7);
-#endif
-	ar0 = fontwidth(p) - 1;
-	mga_outl(M_FXBNDRY, ((xx+ar0)<<16) | xx);
-	if (fontwidth(p) <= 8)
-		step = 1;
-	else if (fontwidth(p) <= 16)
-		step = 2;
-	else
-		step = 4;
-	if (fontwidth(p) == step << 3) {
-		size_t charcell = fontheight(p)*step;
-		/* TODO: Align charcell to 4B for BE */
-		mga_outl(M_DWGCTL, M_DWG_ILOAD | M_DWG_SGNZERO | M_DWG_SHIFTZERO | M_DWG_BMONOWF | M_DWG_LINEAR | M_DWG_REPLACE);
-		mga_outl(M_FCOL, fgx);
-		mga_outl(M_BCOL, bgx);
-		mga_outl(M_AR3, 0);
-		mga_outl(M_AR0, fontheight(p)*fontwidth(p)-1);
-		mga_ydstlen(yy, fontheight(p));
-		mga_memcpy_toio(ACCESS_FBINFO(mmio.vbase), 0, p->fontdata+(c&p->charmask)*charcell, charcell);
-	} else {
-		u8* chardata = p->fontdata+(c&p->charmask)*fontheight(p)*step;
-		int i;
-
-		mga_outl(M_DWGCTL, M_DWG_ILOAD | M_DWG_SGNZERO | M_DWG_SHIFTZERO | M_DWG_BMONOWF | M_DWG_REPLACE);
-		mga_outl(M_FCOL, fgx);
-		mga_outl(M_BCOL, bgx);
-		mga_outl(M_AR5, 0);
-		mga_outl(M_AR3, 0);
-		mga_outl(M_AR0, ar0);
-		mga_ydstlen(yy, fontheight(p));
-
-		switch (step) {
-		case 1:
-			for (i = fontheight(p); i > 0; i--) {
-#ifdef __LITTLE_ENDIAN
-				mga_outl(0, *chardata++);
-#else
-				mga_outl(0, (*chardata++) << 24);
-#endif
-			}
-			break;
-		case 2:
-			for (i = fontheight(p); i > 0; i--) {
-#ifdef __LITTLE_ENDIAN
-				mga_outl(0, *(u_int16_t*)chardata);
-#else
-				mga_outl(0, (*(u_int16_t*)chardata) << 16);
-#endif
-				chardata += 2;
-			}
+	switch (rect->rop) {
+		case ROP_COPY:
+			matroxfb_cfb4_clear(PMINFO ((u_int32_t*)info->pseudo_palette)[rect->color], rect->dy, rect->dx, rect->height, rect->width);
 			break;
-		case 4:
-			mga_memcpy_toio(ACCESS_FBINFO(mmio.vbase), 0, chardata, fontheight(p) * 4);
-			break;
-		}
 	}
-	WaitTillIdle();
-#ifdef __BIG_ENDIAN
-	mga_outl(M_OPMODE, ACCESS_FBINFO(accel.m_opmode));
-#endif
-	CRITEND
 }
 
-#ifdef FBCON_HAS_CFB8
-static void matrox_cfb8_putc(struct vc_data* conp, struct display* p, int c, int yy, int xx) {
-	u_int32_t fgx, bgx;
-	MINFO_FROM_DISP(p);
-
-	DBG_HEAVY("matroxfb_cfb8_putc");
-
-	fgx = attr_fgcol(p, c);
-	bgx = attr_bgcol(p, c);
-	fgx |= (fgx << 8);
-	fgx |= (fgx << 16);
-	bgx |= (bgx << 8);
-	bgx |= (bgx << 16);
-	ACCESS_FBINFO(curr.putc)(fgx, bgx, p, c, yy, xx);
-}
-#endif
-
-#ifdef FBCON_HAS_CFB16
-static void matrox_cfb16_putc(struct vc_data* conp, struct display* p, int c, int yy, int xx) {
-	u_int32_t fgx, bgx;
-	MINFO_FROM_DISP(p);
-
-	DBG_HEAVY("matroxfb_cfb16_putc");
-
-	fgx = ((u_int16_t*)p->dispsw_data)[attr_fgcol(p, c)];
-	bgx = ((u_int16_t*)p->dispsw_data)[attr_bgcol(p, c)];
-	fgx |= (fgx << 16);
-	bgx |= (bgx << 16);
-	ACCESS_FBINFO(curr.putc)(fgx, bgx, p, c, yy, xx);
-}
-#endif
-
-#if defined(FBCON_HAS_CFB32) || defined(FBCON_HAS_CFB24)
-static void matrox_cfb32_putc(struct vc_data* conp, struct display* p, int c, int yy, int xx) {
-	u_int32_t fgx, bgx;
-	MINFO_FROM_DISP(p);
-
-	DBG_HEAVY("matroxfb_cfb32_putc");
-
-	fgx = ((u_int32_t*)p->dispsw_data)[attr_fgcol(p, c)];
-	bgx = ((u_int32_t*)p->dispsw_data)[attr_bgcol(p, c)];
-	ACCESS_FBINFO(curr.putc)(fgx, bgx, p, c, yy, xx);
-}
-#endif
-
-static void matrox_cfbX_fastputcs(u_int32_t fgx, u_int32_t bgx, struct display* p, const unsigned short* s, int count, int yy, int xx) {
-	unsigned int charcell;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	yy *= fontheight(p);
-	xx *= fontwidth(p);
-	charcell = fontwidth(p) * fontheight(p);
-
-	CRITBEGIN
-
-	mga_fifo(3);
-	mga_outl(M_DWGCTL, M_DWG_BITBLT | M_DWG_SGNZERO | M_DWG_SHIFTZERO | M_DWG_BMONOWF | M_DWG_LINEAR | M_DWG_REPLACE);
-	mga_outl(M_FCOL, fgx);
-	mga_outl(M_BCOL, bgx);
-	while (count--) {
-		u_int32_t ar3 = ACCESS_FBINFO(fastfont.mgabase) + (scr_readw(s++) & p->charmask)*charcell;
-
-		mga_fifo(4);
-		mga_outl(M_FXBNDRY, ((xx + fontwidth(p) - 1) << 16) | xx);
-		mga_outl(M_AR3, ar3);
-		mga_outl(M_AR0, (ar3 + charcell - 1) & 0x0003FFFF);
-		mga_ydstlen(yy, fontheight(p));
-		xx += fontwidth(p);
-	}
-	WaitTillIdle();
-
-	CRITEND
-}
-
-static void matrox_cfbX_putcs(u_int32_t fgx, u_int32_t bgx, struct display* p, const unsigned short* s, int count, int yy, int xx) {
+static void matroxfb_1bpp_imageblit(WPMINFO u_int32_t fgx, u_int32_t bgx,
+		const u_int8_t* chardata, int width, int height, int yy, int xx) {
 	u_int32_t step;
 	u_int32_t ydstlen;
 	u_int32_t xlen;
@@ -565,26 +394,18 @@ static void matrox_cfbX_putcs(u_int32_t fgx, u_int32_t bgx, struct display* p, c
 	vaddr_t mmio;
 	int easy;
 	CRITFLAGS
-	MINFO_FROM_DISP(p);
 
-	DBG_HEAVY("matroxfb_cfbX_putcs");
+	DBG_HEAVY(__FUNCTION__);
 
-	yy *= fontheight(p);
-	xx *= fontwidth(p);
-	if (fontwidth(p) <= 8)
-		step = 1;
-	else if (fontwidth(p) <= 16)
-		step = 2;
-	else
-		step = 4;
-	charcell = fontheight(p)*step;
+	step = (width + 7) >> 3;
+	charcell = height * step;
 	xlen = (charcell + 3) & ~3;
-	ydstlen = (yy << 16) | fontheight(p);
-	if (fontwidth(p) == step << 3) {
-		ar0 = fontheight(p)*fontwidth(p) - 1;
+	ydstlen = (yy << 16) | height;
+	if (width == step << 3) {
+		ar0 = height * width - 1;
 		easy = 1;
 	} else {
-		ar0 = fontwidth(p) - 1;
+		ar0 = width - 1;
 		easy = 0;
 	}
 
@@ -602,50 +423,55 @@ static void matrox_cfbX_putcs(u_int32_t fgx, u_int32_t bgx, struct display* p, c
 		mga_outl(M_DWGCTL, M_DWG_ILOAD | M_DWG_SGNZERO | M_DWG_SHIFTZERO | M_DWG_BMONOWF | M_DWG_REPLACE);
 	mga_outl(M_FCOL, fgx);
 	mga_outl(M_BCOL, bgx);
-	fxbndry = ((xx + fontwidth(p) - 1) << 16) | xx;
+	fxbndry = ((xx + width - 1) << 16) | xx;
 	mmio = ACCESS_FBINFO(mmio.vbase);
-	while (count--) {
-		u_int8_t* chardata = p->fontdata + (scr_readw(s++) & p->charmask)*charcell;
 
-		mga_fifo(6);
-		mga_writel(mmio, M_FXBNDRY, fxbndry);
-		mga_writel(mmio, M_AR0, ar0);
-		mga_writel(mmio, M_AR3, 0);
-		if (easy) {
-			mga_writel(mmio, M_YDSTLEN | M_EXEC, ydstlen);
-			mga_memcpy_toio(mmio, 0, chardata, xlen);
-		} else {
-			mga_writel(mmio, M_AR5, 0);
-			mga_writel(mmio, M_YDSTLEN | M_EXEC, ydstlen);
-			switch (step) {
-				case 1:	{
-						u_int8_t* charend = chardata + charcell;
-						for (; chardata != charend; chardata++) {
+	mga_fifo(6);
+	mga_writel(mmio, M_FXBNDRY, fxbndry);
+	mga_writel(mmio, M_AR0, ar0);
+	mga_writel(mmio, M_AR3, 0);
+	if (easy) {
+		mga_writel(mmio, M_YDSTLEN | M_EXEC, ydstlen);
+		mga_memcpy_toio(mmio, 0, chardata, xlen);
+	} else {
+		mga_writel(mmio, M_AR5, 0);
+		mga_writel(mmio, M_YDSTLEN | M_EXEC, ydstlen);
+		if ((step & 3) == 0) {
+			/* Great. Source has 32bit aligned lines, so we can feed them
+			   directly to the accelerator. */
+			mga_memcpy_toio(mmio, 0, chardata, charcell);
+		} else if (step == 1) {
+			/* Special case for 1..8bit widths */
+			while (height--) {
 #ifdef __LITTLE_ENDIAN
-							mga_writel(mmio, 0, *chardata);
+				mga_writel(mmio, 0, *chardata);
 #else
-							mga_writel(mmio, 0, (*chardata) << 24);
+				mga_writel(mmio, 0, (*chardata) << 24);
 #endif
-						}
-					}
-					break;
-				case 2:	{
-						u_int8_t* charend = chardata + charcell;
-						for (; chardata != charend; chardata += 2) {
+				chardata++;
+			}
+		} else if (step == 2) {
+			/* Special case for 9..15bit widths */
+			while (height--) {
 #ifdef __LITTLE_ENDIAN
-							mga_writel(mmio, 0, *(u_int16_t*)chardata);
+				mga_writel(mmio, 0, *(u_int16_t*)chardata);
 #else
-							mga_writel(mmio, 0, (*(u_int16_t*)chardata) << 16);
+				mga_writel(mmio, 0, (*(u_int16_t*)chardata) << 16);
 #endif
-						}
-					}
-					break;
-				default:
-					mga_memcpy_toio(mmio, 0, chardata, charcell);
-					break;
+				chardata += 2;
+			}
+		} else {
+			/* Tell... well, why bother... */
+			while (height--) {
+				size_t i;
+				
+				for (i = 0; i < step; i += 4) {
+					/* Hope that there are at least three readable bytes beyond the end of bitmap */
+					mga_writel(mmio, 0, get_unaligned((u_int32_t*)(chardata + i)));
+				}
+				chardata += step;
 			}
 		}
-		fxbndry += fontwidth(p) + (fontwidth(p) << 16);
 	}
 	WaitTillIdle();
 #ifdef __BIG_ENDIAN
@@ -654,606 +480,26 @@ static void matrox_cfbX_putcs(u_int32_t fgx, u_int32_t bgx, struct display* p, c
 	CRITEND
 }
 
-#ifdef FBCON_HAS_CFB8
-static void matrox_cfb8_putcs(struct vc_data* conp, struct display* p, const unsigned short* s, int count, int yy, int xx) {
-	u_int16_t c;
-	u_int32_t fgx, bgx;
-	MINFO_FROM_DISP(p);
-
-	DBG_HEAVY("matroxfb_cfb8_putcs");
-
-	c = scr_readw(s);
-	fgx = attr_fgcol(p, c);
-	bgx = attr_bgcol(p, c);
-	fgx |= (fgx << 8);
-	fgx |= (fgx << 16);
-	bgx |= (bgx << 8);
-	bgx |= (bgx << 16);
-	ACCESS_FBINFO(curr.putcs)(fgx, bgx, p, s, count, yy, xx);
-}
-#endif
-
-#ifdef FBCON_HAS_CFB16
-static void matrox_cfb16_putcs(struct vc_data* conp, struct display* p, const unsigned short* s, int count, int yy, int xx) {
-	u_int16_t c;
-	u_int32_t fgx, bgx;
-	MINFO_FROM_DISP(p);
-
-	DBG_HEAVY("matroxfb_cfb16_putcs");
-
-	c = scr_readw(s);
-	fgx = ((u_int16_t*)p->dispsw_data)[attr_fgcol(p, c)];
-	bgx = ((u_int16_t*)p->dispsw_data)[attr_bgcol(p, c)];
-	fgx |= (fgx << 16);
-	bgx |= (bgx << 16);
-	ACCESS_FBINFO(curr.putcs)(fgx, bgx, p, s, count, yy, xx);
-}
-#endif
-
-#if defined(FBCON_HAS_CFB32) || defined(FBCON_HAS_CFB24)
-static void matrox_cfb32_putcs(struct vc_data* conp, struct display* p, const unsigned short* s, int count, int yy, int xx) {
-	u_int16_t c;
-	u_int32_t fgx, bgx;
-	MINFO_FROM_DISP(p);
-
-	DBG_HEAVY("matroxfb_cfb32_putcs");
-
-	c = scr_readw(s);
-	fgx = ((u_int32_t*)p->dispsw_data)[attr_fgcol(p, c)];
-	bgx = ((u_int32_t*)p->dispsw_data)[attr_bgcol(p, c)];
-	ACCESS_FBINFO(curr.putcs)(fgx, bgx, p, s, count, yy, xx);
-}
-#endif
-
-#ifdef FBCON_HAS_CFB4
-static void matrox_cfb4_revc(struct display* p, int xx, int yy) {
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	DBG_LOOP("matroxfb_cfb4_revc");
-
-	if (fontwidth(p) & 1) {
-		fbcon_cfb4_revc(p, xx, yy);
-		return;
-	}
-	yy *= fontheight(p);
-	xx *= fontwidth(p);
-	xx |= (xx + fontwidth(p)) << 16;
-	xx >>= 1;
-
-	CRITBEGIN
-
-	mga_fifo(5);
-	mga_outl(M_DWGCTL, ACCESS_FBINFO(accel.m_dwg_rect) | M_DWG_XOR);
-	mga_outl(M_FCOL, 0xFFFFFFFF);
-	mga_outl(M_FXBNDRY, xx);
-	mga_outl(M_YDST, yy * p->var.xres_virtual >> 6);
-	mga_outl(M_LEN | M_EXEC, fontheight(p));
-	WaitTillIdle();
-
-	CRITEND
-}
-#endif
-
-#ifdef FBCON_HAS_CFB8
-static void matrox_cfb8_revc(struct display* p, int xx, int yy) {
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	DBG_LOOP("matrox_cfb8_revc")
-
-	yy *= fontheight(p);
-	xx *= fontwidth(p);
-
-	CRITBEGIN
-
-	mga_fifo(4);
-	mga_outl(M_DWGCTL, ACCESS_FBINFO(accel.m_dwg_rect) | M_DWG_XOR);
-	mga_outl(M_FCOL, 0x0F0F0F0F);
-	mga_outl(M_FXBNDRY, ((xx + fontwidth(p)) << 16) | xx);
-	mga_ydstlen(yy, fontheight(p));
-	WaitTillIdle();
-
-	CRITEND
-}
-#endif
-
-#if defined(FBCON_HAS_CFB16) || defined(FBCON_HAS_CFB24) || defined(FBCON_HAS_CFB32)
-static void matrox_cfbX_revc(struct display* p, int xx, int yy) {
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	DBG_LOOP("matrox_cfbX_revc")
-
-	yy *= fontheight(p);
-	xx *= fontwidth(p);
-
-	CRITBEGIN
-
-	mga_fifo(4);
-	mga_outl(M_DWGCTL, ACCESS_FBINFO(accel.m_dwg_rect) | M_DWG_XOR);
-	mga_outl(M_FCOL, 0xFFFFFFFF);
-	mga_outl(M_FXBNDRY, ((xx + fontwidth(p)) << 16) | xx);
-	mga_ydstlen(yy, fontheight(p));
-	WaitTillIdle();
-
-	CRITEND
-}
-#endif
-
-static void matrox_cfbX_clear_margins(struct vc_data* conp, struct display* p, int bottom_only) {
-	unsigned int bottom_height, right_width;
-	unsigned int bottom_start, right_start;
-	unsigned int cell_h, cell_w;
-
-	DBG("matrox_cfbX_clear_margins")
-
-	cell_w = fontwidth(p);
-	if (!cell_w) return;	/* PARANOID */
-	right_width = p->var.xres % cell_w;
-	right_start = p->var.xres - right_width;
-	if (!bottom_only && right_width) {
-		/* clear whole right margin, not only visible portion */
-		matroxfb_accel_clear(     PMXINFO(p)
-			     /* color */  0x00000000,
-			     /* y */      0,
-			     /* x */      p->var.xoffset + right_start,
-			     /* height */ p->var.yres_virtual,
-			     /* width */  right_width);
-	}
-	cell_h = fontheight(p);
-	if (!cell_h) return;	/* PARANOID */
-	bottom_height = p->var.yres % cell_h;
-	if (bottom_height) {
-		bottom_start = p->var.yres - bottom_height;
-		matroxfb_accel_clear(		  PMXINFO(p)
-				     /* color */  0x00000000,
-				     /* y */	  p->var.yoffset + bottom_start,
-				     /* x */	  p->var.xoffset,
-				     /* height */ bottom_height,
-				     /* width */  right_start);
-	}
-}
-
-static void matrox_text_setup(struct display* p) {
-	MINFO_FROM_DISP(p);
-
-	p->next_line = ACCESS_FBINFO(fbcon).fix.line_length ? ACCESS_FBINFO(fbcon).fix.line_length : ((p->var.xres_virtual / (fontwidth(p)?fontwidth(p):8)) * ACCESS_FBINFO(devflags.textstep));
-	p->next_plane = 0;
-}
-
-static void matrox_text_bmove(struct display* p, int sy, int sx, int dy, int dx,
-		int height, int width) {
-	unsigned int srcoff;
-	unsigned int dstoff;
-	unsigned int step;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	CRITBEGIN
-
-	step = ACCESS_FBINFO(devflags.textstep);
-	srcoff = (sy * p->next_line) + (sx * step);
-	dstoff = (dy * p->next_line) + (dx * step);
-	if (dstoff < srcoff) {
-		while (height > 0) {
-			int i;
-			for (i = width; i > 0; dstoff += step, srcoff += step, i--)
-				mga_writew(ACCESS_FBINFO(video.vbase), dstoff, mga_readw(ACCESS_FBINFO(video.vbase), srcoff));
-			height--;
-			dstoff += p->next_line - width * step;
-			srcoff += p->next_line - width * step;
-		}
-	} else {
-		unsigned int off;
-
-		off = (height - 1) * p->next_line + (width - 1) * step;
-		srcoff += off;
-		dstoff += off;
-		while (height > 0) {
-			int i;
-			for (i = width; i > 0; dstoff -= step, srcoff -= step, i--)
-				mga_writew(ACCESS_FBINFO(video.vbase), dstoff, mga_readw(ACCESS_FBINFO(video.vbase), srcoff));
-			dstoff -= p->next_line - width * step;
-			srcoff -= p->next_line - width * step;
-			height--;
-		}
-	}
-	CRITEND
-}
-
-static void matrox_text_clear(struct vc_data* conp, struct display* p, int sy, int sx,
-		int height, int width) {
-	unsigned int offs;
-	unsigned int val;
-	unsigned int step;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	step = ACCESS_FBINFO(devflags.textstep);
-	offs = sy * p->next_line + sx * step;
-	val = ntohs((attr_bgcol(p, conp->vc_video_erase_char) << 4) | attr_fgcol(p, conp->vc_video_erase_char) | (' ' << 8));
-
-	CRITBEGIN
-
-	while (height > 0) {
-		int i;
-		for (i = width; i > 0; offs += step, i--)
-			mga_writew(ACCESS_FBINFO(video.vbase), offs, val);
-		offs += p->next_line - width * step;
-		height--;
-	}
-	CRITEND
-}
-
-static void matrox_text_putc(struct vc_data* conp, struct display* p, int c, int yy, int xx) {
-	unsigned int offs;
-	unsigned int chr;
-	unsigned int step;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	step = ACCESS_FBINFO(devflags.textstep);
-	offs = yy * p->next_line + xx * step;
-	chr = attr_fgcol(p,c) | (attr_bgcol(p,c) << 4) | ((c & p->charmask) << 8);
-	if (chr & 0x10000) chr |= 0x08;
-
-	CRITBEGIN
-
-	mga_writew(ACCESS_FBINFO(video.vbase), offs, ntohs(chr));
-
-	CRITEND
-}
-
-static void matrox_text_putcs(struct vc_data* conp, struct display* p, const unsigned short* s,
-		int count, int yy, int xx) {
-	unsigned int offs;
-	unsigned int attr;
-	unsigned int step;
-	u_int16_t c;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	step = ACCESS_FBINFO(devflags.textstep);
-	offs = yy * p->next_line + xx * step;
-	c = scr_readw(s);
-	attr = attr_fgcol(p, c) | (attr_bgcol(p, c) << 4);
-
-	CRITBEGIN
-
-	while (count-- > 0) {
-		unsigned int chr = ((scr_readw(s++)) & p->charmask) << 8;
-		if (chr & 0x10000) chr ^= 0x10008;
-		mga_writew(ACCESS_FBINFO(video.vbase), offs, ntohs(attr|chr));
-		offs += step;
-	}
-
-	CRITEND
-}
-
-static void matrox_text_revc(struct display* p, int xx, int yy) {
-	unsigned int offs;
-	unsigned int step;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	step = ACCESS_FBINFO(devflags.textstep);
-	offs = yy * p->next_line + xx * step + 1;
-
-	CRITBEGIN
-
-	mga_writeb(ACCESS_FBINFO(video.vbase), offs, mga_readb(ACCESS_FBINFO(video.vbase), offs) ^ 0x77);
-
-	CRITEND
-}
-
-static void matrox_text_createcursor(WPMINFO struct display* p) {
-	CRITFLAGS
-
-	if (ACCESS_FBINFO(currcon_display) != p)
-		return;
-
-	matroxfb_createcursorshape(PMINFO p, 0);
-
-	CRITBEGIN
-
-	mga_setr(M_CRTC_INDEX, 0x0A, ACCESS_FBINFO(cursor.u));
-	mga_setr(M_CRTC_INDEX, 0x0B, ACCESS_FBINFO(cursor.d) - 1);
-
-	CRITEND
-}
-
-static void matrox_text_cursor(struct display* p, int mode, int x, int y) {
-	unsigned int pos;
-	CRITFLAGS
-	MINFO_FROM_DISP(p);
-
-	if (ACCESS_FBINFO(currcon_display) != p)
-		return;
-
-	if (mode == CM_ERASE) {
-		if (ACCESS_FBINFO(cursor.state) != CM_ERASE) {
-
-			CRITBEGIN
-
-			mga_setr(M_CRTC_INDEX, 0x0A, 0x20);
-
-			CRITEND
-
-			ACCESS_FBINFO(cursor.state) = CM_ERASE;
-		}
-		return;
-	}
-	if ((p->conp->vc_cursor_type & CUR_HWMASK) != ACCESS_FBINFO(cursor.type))
-		matrox_text_createcursor(PMINFO p);
-
-	/* DO NOT CHECK cursor.x != x because of matroxfb_vgaHWinit moves cursor to 0,0 */
-	ACCESS_FBINFO(cursor.x) = x;
-	ACCESS_FBINFO(cursor.y) = y;
-	pos = p->next_line / ACCESS_FBINFO(devflags.textstep) * y + x;
-
-	CRITBEGIN
-
-	mga_setr(M_CRTC_INDEX, 0x0F, pos);
-	mga_setr(M_CRTC_INDEX, 0x0E, pos >> 8);
-
-	mga_setr(M_CRTC_INDEX, 0x0A, ACCESS_FBINFO(cursor.u));
-
-	CRITEND
-
-	ACCESS_FBINFO(cursor.state) = CM_DRAW;
-}
-
-void matrox_text_round(CPMINFO struct fb_var_screeninfo* var, struct display* p) {
-	unsigned hf;
-	unsigned vf;
-	unsigned vxres;
-	unsigned ych;
-
-	hf = fontwidth(p);
-	if (!hf) hf = 8;
-	/* do not touch xres */
-	vxres = (var->xres_virtual + hf - 1) / hf;
-	if (vxres >= 256)
-		vxres = 255;
-	if (vxres < 16)
-		vxres = 16;
-	vxres = (vxres + 1) & ~1;	/* must be even */
-	vf = fontheight(p);
-	if (!vf) vf = 16;
-	if (var->yres < var->yres_virtual) {
-		ych = ACCESS_FBINFO(devflags.textvram) / vxres;
-		var->yres_virtual = ych * vf;
-	} else
-		ych = var->yres_virtual / vf;
-	if (vxres * ych > ACCESS_FBINFO(devflags.textvram)) {
-		ych = ACCESS_FBINFO(devflags.textvram) / vxres;
-		var->yres_virtual = ych * vf;
-	}
-	var->xres_virtual = vxres * hf;
-}
-
-EXPORT_SYMBOL(matrox_text_round);
-
-static int matrox_text_setfont(struct display* p, int width, int height) {
-	DBG("matrox_text_setfont");
-
-	if (p) {
-		MINFO_FROM_DISP(p);
-
-		matrox_text_round(PMINFO &p->var, p);
-		p->next_line = ACCESS_FBINFO(fbcon).fix.line_length = ((p->var.xres_virtual / (fontwidth(p)?fontwidth(p):8)) * ACCESS_FBINFO(devflags.textstep));
-
-		if (p->conp)
-			matrox_text_createcursor(PMINFO p);
-	}
-	return 0;
-}
-
-#define matrox_cfb16_revc matrox_cfbX_revc
-#define matrox_cfb24_revc matrox_cfbX_revc
-#define matrox_cfb32_revc matrox_cfbX_revc
-
-#define matrox_cfb24_clear matrox_cfb32_clear
-#define matrox_cfb24_putc matrox_cfb32_putc
-#define matrox_cfb24_putcs matrox_cfb32_putcs
-
-#ifdef FBCON_HAS_VGATEXT
-static struct display_switch matroxfb_text = {
-	.setup =	matrox_text_setup,
-	.bmove =	matrox_text_bmove,
-	.clear =	matrox_text_clear,
-	.putc =		matrox_text_putc,
-	.putcs =	matrox_text_putcs,
-	.revc =		matrox_text_revc,
-	.cursor =	matrox_text_cursor,
-	.set_font =	matrox_text_setfont,
-	.fontwidthmask =FONTWIDTH(8)|FONTWIDTH(9)
-};
-#endif
-
-#ifdef FBCON_HAS_CFB4
-static struct display_switch matroxfb_cfb4 = {
-	.setup =	fbcon_cfb4_setup,
-	.bmove =	matrox_cfb4_bmove,
-	.clear =	matrox_cfb4_clear,
-	.putc =		fbcon_cfb4_putc,
-	.putcs =	fbcon_cfb4_putcs,
-	.revc =		matrox_cfb4_revc,
-	.fontwidthmask =FONTWIDTH(8) /* fix, fix, fix it */
-};
-#endif
-
-#ifdef FBCON_HAS_CFB8
-static struct display_switch matroxfb_cfb8 = {
-	.setup =	fbcon_cfb8_setup,
-	.bmove =	matrox_cfbX_bmove,
-	.clear =	matrox_cfb8_clear,
-	.putc =		matrox_cfb8_putc,
-	.putcs =	matrox_cfb8_putcs,
-	.revc =		matrox_cfb8_revc,
-	.clear_margins =matrox_cfbX_clear_margins,
-	.fontwidthmask =~1 /* FONTWIDTHS */
-};
-#endif
-
-#ifdef FBCON_HAS_CFB16
-static struct display_switch matroxfb_cfb16 = {
-	.setup =	fbcon_cfb16_setup,
-	.bmove =	matrox_cfbX_bmove,
-	.clear =	matrox_cfb16_clear,
-	.putc =		matrox_cfb16_putc,
-	.putcs =	matrox_cfb16_putcs,
-	.revc =		matrox_cfb16_revc,
-	.clear_margins =matrox_cfbX_clear_margins,
-	.fontwidthmask =~1 /* FONTWIDTHS */
-};
-#endif
 
-#ifdef FBCON_HAS_CFB24
-static struct display_switch matroxfb_cfb24 = {
-	.setup =	fbcon_cfb24_setup,
-	.bmove =	matrox_cfbX_bmove,
-	.clear =	matrox_cfb24_clear,
-	.putc =		matrox_cfb24_putc,
-	.putcs =	matrox_cfb24_putcs,
-	.revc =		matrox_cfb24_revc,
-	.clear_margins =matrox_cfbX_clear_margins,
-	.fontwidthmask =~1 /* FONTWIDTHS */ /* TODO: and what about non-aligned access on BE? I think that there are no in my code */
-};
-#endif
+static void matroxfb_imageblit(struct fb_info* info, const struct fb_image* image) {
+	MINFO_FROM_INFO(info);
 
-#ifdef FBCON_HAS_CFB32
-static struct display_switch matroxfb_cfb32 = {
-	.setup =	fbcon_cfb32_setup,
-	.bmove =	matrox_cfbX_bmove,
-	.clear =	matrox_cfb32_clear,
-	.putc =		matrox_cfb32_putc,
-	.putcs =	matrox_cfb32_putcs,
-	.revc =		matrox_cfb32_revc,
-	.clear_margins =matrox_cfbX_clear_margins,
-	.fontwidthmask =~1 /* FONTWIDTHS */
-};
-#endif
+	DBG_HEAVY(__FUNCTION__);
 
-void initMatrox(WPMINFO struct display* p) {
-	struct display_switch *swtmp;
+	if (image->depth == 0) {
+		u_int32_t fgx, bgx;
 
-	DBG("initMatrox")
-
-	if (ACCESS_FBINFO(currcon_display) != p)
-		return;
-	if (p->dispsw && p->conp)
-		fb_con.con_cursor(p->conp, CM_ERASE);
-	p->dispsw_data = NULL;
-	if ((ACCESS_FBINFO(fbcon).var.accel_flags & FB_ACCELF_TEXT) != FB_ACCELF_TEXT) {
-		if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-			swtmp = &matroxfb_text;
-		} else {
-			switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
-#ifdef FBCON_HAS_CFB4
-			case 4:
-				swtmp = &fbcon_cfb4;
-				break;
-#endif
-#ifdef FBCON_HAS_CFB8
-			case 8:
-				swtmp = &fbcon_cfb8;
-				break;
-#endif
-#ifdef FBCON_HAS_CFB16
-			case 16:
-				p->dispsw_data = &ACCESS_FBINFO(cmap.cfb16);
-				swtmp = &fbcon_cfb16;
-				break;
-#endif
-#ifdef FBCON_HAS_CFB24
-			case 24:
-				p->dispsw_data = &ACCESS_FBINFO(cmap.cfb24);
-				swtmp = &fbcon_cfb24;
-				break;
-#endif
-#ifdef FBCON_HAS_CFB32
-			case 32:
-				p->dispsw_data = &ACCESS_FBINFO(cmap.cfb32);
-				swtmp = &fbcon_cfb32;
-				break;
-#endif
-			default:
-				p->dispsw = &fbcon_dummy;
-				return;
-			}
-		}
-		dprintk(KERN_INFO "matroxfb: acceleration disabled\n");
-	} else if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-		swtmp = &matroxfb_text;
+		fgx = ((u_int32_t*)info->pseudo_palette)[image->fg_color];
+		bgx = ((u_int32_t*)info->pseudo_palette)[image->bg_color];
+		matroxfb_1bpp_imageblit(PMINFO fgx, bgx, image->data, image->width, image->height, image->dy, image->dx);
 	} else {
-		switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
-#ifdef FBCON_HAS_CFB4
-		case 4:
-			swtmp = &matroxfb_cfb4;
-			break;
-#endif
-#ifdef FBCON_HAS_CFB8
-		case 8:
-			swtmp = &matroxfb_cfb8;
-			break;
-#endif
-#ifdef FBCON_HAS_CFB16
-		case 16:
-			p->dispsw_data = &ACCESS_FBINFO(cmap.cfb16);
-			swtmp = &matroxfb_cfb16;
-			break;
-#endif
-#ifdef FBCON_HAS_CFB24
-		case 24:
-			p->dispsw_data = &ACCESS_FBINFO(cmap.cfb24);
-			swtmp = &matroxfb_cfb24;
-			break;
-#endif
-#ifdef FBCON_HAS_CFB32
-		case 32:
-			p->dispsw_data = &ACCESS_FBINFO(cmap.cfb32);
-			swtmp = &matroxfb_cfb32;
-			break;
-#endif
-		default:
-			p->dispsw = &fbcon_dummy;
-			return;
-		}
-	}
-	memcpy(&ACCESS_FBINFO(dispsw), swtmp, sizeof(ACCESS_FBINFO(dispsw)));
-	p->dispsw = &ACCESS_FBINFO(dispsw);
-	if ((ACCESS_FBINFO(fbcon).fix.type != FB_TYPE_TEXT) && ACCESS_FBINFO(devflags.hwcursor)) {
-		ACCESS_FBINFO(hw_switch)->selhwcursor(PMINFO2);
-	}
-}
-
-EXPORT_SYMBOL(initMatrox);
-
-void matrox_init_putc(WPMINFO struct display* p, void (*dac_createcursor)(WPMINFO struct display* p)) {
-	int i;
-
-	if (p && p->conp) {
-		if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-			matrox_text_createcursor(PMINFO p);
-			matrox_text_loadfont(PMINFO p);
-			i = 0;
-		} else {
-			dac_createcursor(PMINFO p);
-			i = matroxfb_fastfont_tryset(PMINFO p);
-		}
-	} else
-		i = 0;
-	if (i) {
-		ACCESS_FBINFO(curr.putc) = matrox_cfbX_fastputc;
-		ACCESS_FBINFO(curr.putcs) = matrox_cfbX_fastputcs;
-	} else {
-		ACCESS_FBINFO(curr.putc) = matrox_cfbX_putc;
-		ACCESS_FBINFO(curr.putcs) = matrox_cfbX_putcs;
+		/* Danger! image->depth is useless: logo painting code always
+		   passes framebuffer color depth here, although logo data are
+		   always 8bpp and info->pseudo_palette is changed to contain
+		   logo palette to be used (but only for true/direct-color... sic...).
+		   So do it completely in software... */
+		cfb_imageblit(info, image);
 	}
 }
 
-EXPORT_SYMBOL(matrox_init_putc);
-
 MODULE_LICENSE("GPL");

drivers/video/matrox/matroxfb_accel.h

@@ -3,9 +3,6 @@
 
 #include "matroxfb_base.h"
 
-void matrox_init_putc(WPMINFO struct display* p, void (*)(WPMINFO struct display *p));
 void matrox_cfbX_init(WPMINFO2);
-void matrox_text_round(CPMINFO struct fb_var_screeninfo* var, struct display* p);
-void initMatrox(WPMINFO struct display* p);
 
 #endif

drivers/video/matrox/matroxfb_base.c

@@ -101,6 +101,7 @@
 
 /* make checkconfig does not check included files... */
 #include <linux/config.h>
+#include <linux/version.h>
 
 #include "matroxfb_base.h"
 #include "matroxfb_misc.h"
@@ -111,9 +112,10 @@
 #include "matroxfb_crtc2.h"
 #include "matroxfb_g450.h"
 #include <linux/matroxfb.h>
+#include <linux/interrupt.h>
 #include <asm/uaccess.h>
 
-#ifdef CONFIG_PPC
+#ifdef CONFIG_ALL_PPC
 unsigned char nvram_read_byte(int);
 static int default_vmode = VMODE_NVRAM;
 static int default_cmode = CMODE_NVRAM;
@@ -145,25 +147,136 @@ static struct fb_var_screeninfo vesafb_defined = {
 	39721L,48L,16L,33L,10L,
 	96L,2L,~0,	/* No sync info */
 	FB_VMODE_NONINTERLACED,
-	{0,0,0,0,0,0}
+	0, {0,0,0,0,0}
 };
 
 
 
 /* --------------------------------------------------------------------- */
-static inline void my_install_cmap(WPMINFO2)
+static void update_crtc2(WPMINFO unsigned int pos) {
+	struct matroxfb_dh_fb_info* info = ACCESS_FBINFO(crtc2.info);
+
+	/* Make sure that displays are compatible */
+	if (info && (info->fbcon.var.bits_per_pixel == ACCESS_FBINFO(fbcon).var.bits_per_pixel)
+	         && (info->fbcon.var.xres_virtual == ACCESS_FBINFO(fbcon).var.xres_virtual)
+	         && (info->fbcon.var.green.length == ACCESS_FBINFO(fbcon).var.green.length)
+	         ) {
+		switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
+			case 16:
+			case 32:
+				pos = pos * 8;
+				if (info->interlaced) {
+					mga_outl(0x3C2C, pos);
+					mga_outl(0x3C28, pos + ACCESS_FBINFO(fbcon).var.xres_virtual * ACCESS_FBINFO(fbcon).var.bits_per_pixel / 8);
+				} else {
+					mga_outl(0x3C28, pos);
+				}
+				break;
+		}
+	}
+}
+
+static irqreturn_t matrox_irq(int irq, void *dev_id, struct pt_regs *fp)
 {
-	/* Do not touch this code if you do not understand what it does! */
-	/* Never try to use do_install_cmap() instead. It is crap. */
-	struct fb_cmap* cmap = &ACCESS_FBINFO(currcon_display)->cmap;
+	u_int32_t status;
+	int handled = 0;
+
+	MINFO_FROM(dev_id);
 
-	if (cmap->len)
-		fb_set_cmap(cmap, 1, &ACCESS_FBINFO(fbcon));
+	status = mga_inl(M_STATUS);
+
+	if (status & 0x20) {
+		mga_outl(M_ICLEAR, 0x20);
+		ACCESS_FBINFO(crtc1.vsync.cnt)++;
+		wake_up_interruptible(&ACCESS_FBINFO(crtc1.vsync.wait));
+		handled = 1;
+	}
+	if (status & 0x200) {
+		mga_outl(M_ICLEAR, 0x200);
+		ACCESS_FBINFO(crtc2.vsync.cnt)++;
+		wake_up_interruptible(&ACCESS_FBINFO(crtc2.vsync.wait));
+		handled = 1;
+	}
+	return IRQ_RETVAL(handled);
+}
+
+int matroxfb_enable_irq(WPMINFO int reenable) {
+	u_int32_t bm;
+	
+	if (ACCESS_FBINFO(devflags.accelerator) == FB_ACCEL_MATROX_MGAG400)
+		bm = 0x220;
 	else
-		fb_set_cmap(fb_default_cmap(ACCESS_FBINFO(curr.cmap_len)),
-			    1, &ACCESS_FBINFO(fbcon));
+		bm = 0x020;
+
+	if (!test_and_set_bit(0, &ACCESS_FBINFO(irq_flags))) {
+		printk(KERN_DEBUG "matroxfb: enabling IRQ\n");
+		if (request_irq(ACCESS_FBINFO(pcidev)->irq, matrox_irq,
+				SA_SHIRQ, "MGA Vertical Sync", MINFO)) {
+			clear_bit(0, &ACCESS_FBINFO(irq_flags));
+			return -EINVAL;
+		}
+		mga_outl(M_IEN, mga_inl(M_IEN) | bm);
+	} else if (reenable) {
+		u_int32_t ien;
+		
+		ien = mga_inl(M_IEN);
+		if ((ien & bm) != bm) {
+			printk(KERN_DEBUG "matroxfb: someone disabled IRQ [%08X]\n", ien);
+			mga_outl(M_IEN, ien | bm);
+		}
+	}
+	return 0;
+}
+
+static void matroxfb_disable_irq(WPMINFO2) {
+	if (test_and_clear_bit(0, &ACCESS_FBINFO(irq_flags))) {
+		printk(KERN_DEBUG "matroxfb: disabling IRQ\n");
+		if (ACCESS_FBINFO(devflags.accelerator) == FB_ACCEL_MATROX_MGAG400)
+			mga_outl(M_IEN, mga_inl(M_IEN) & ~0x220);
+		else
+			mga_outl(M_IEN, mga_inl(M_IEN) & ~0x20);
+		free_irq(ACCESS_FBINFO(pcidev)->irq, MINFO);
+	}
 }
 
+int matroxfb_wait_for_sync(WPMINFO u_int32_t crtc) {
+	wait_queue_t __wait;
+	struct matrox_vsync *vs;
+	unsigned int cnt;
+	int ret;
+
+	switch (crtc) {
+		case 0:
+			vs = &ACCESS_FBINFO(crtc1.vsync);
+			break;
+		case 1:
+			if (ACCESS_FBINFO(devflags.accelerator) != FB_ACCEL_MATROX_MGAG400) {
+				return -ENODEV;
+			}
+			vs = &ACCESS_FBINFO(crtc2.vsync);
+			break;
+		default:
+			return -ENODEV;
+	}
+	ret = matroxfb_enable_irq(PMINFO 0);
+	if (ret) {
+		return ret;
+	}
+        init_waitqueue_entry(&__wait, current);
+
+	cnt = vs->cnt;
+	ret = wait_event_interruptible_timeout(vs->wait, cnt != vs->cnt, HZ/10);
+	if (ret < 0) {
+		return ret;
+	}
+	if (ret == 0) {
+		matroxfb_enable_irq(PMINFO 1);
+		return -ETIMEDOUT;
+	}
+	return 0;
+}
+
+/* --------------------------------------------------------------------- */
 
 static void matrox_pan_var(WPMINFO struct fb_var_screeninfo *var) {
 	unsigned int pos;
@@ -171,23 +284,17 @@ static void matrox_pan_var(WPMINFO struct fb_var_screeninfo *var) {
 #ifdef CONFIG_FB_MATROX_32MB
 	unsigned int p3;
 #endif
-	struct display *disp;
 	CRITFLAGS
 
-	DBG("matrox_pan_var")
+	DBG(__FUNCTION__)
 
 	if (ACCESS_FBINFO(dead))
 		return;
 
 	ACCESS_FBINFO(fbcon).var.xoffset = var->xoffset;
 	ACCESS_FBINFO(fbcon).var.yoffset = var->yoffset;
-	disp = ACCESS_FBINFO(currcon_display);
-	if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-		pos = ACCESS_FBINFO(fbcon).var.yoffset / fontheight(disp) * disp->next_line / ACCESS_FBINFO(devflags.textstep) + ACCESS_FBINFO(fbcon).var.xoffset / (fontwidth(disp)?fontwidth(disp):8);
-	} else {
-		pos = (ACCESS_FBINFO(fbcon).var.yoffset * ACCESS_FBINFO(fbcon).var.xres_virtual + ACCESS_FBINFO(fbcon).var.xoffset) * ACCESS_FBINFO(curr.final_bppShift) / 32;
-		pos += ACCESS_FBINFO(curr.ydstorg.chunks);
-	}
+	pos = (ACCESS_FBINFO(fbcon).var.yoffset * ACCESS_FBINFO(fbcon).var.xres_virtual + ACCESS_FBINFO(fbcon).var.xoffset) * ACCESS_FBINFO(curr.final_bppShift) / 32;
+	pos += ACCESS_FBINFO(curr.ydstorg.chunks);
 	p0 = ACCESS_FBINFO(hw).CRTC[0x0D] = pos & 0xFF;
 	p1 = ACCESS_FBINFO(hw).CRTC[0x0C] = (pos & 0xFF00) >> 8;
 	p2 = ACCESS_FBINFO(hw).CRTCEXT[0] = (ACCESS_FBINFO(hw).CRTCEXT[0] & 0xB0) | ((pos >> 16) & 0x0F) | ((pos >> 14) & 0x40);
@@ -204,6 +311,8 @@ static void matrox_pan_var(WPMINFO struct fb_var_screeninfo *var) {
 		mga_setr(M_EXTVGA_INDEX, 0x08, p3);
 #endif
 	mga_setr(M_EXTVGA_INDEX, 0x00, p2);
+	
+	update_crtc2(PMINFO pos);
 
 	CRITEND
 }
@@ -225,7 +334,6 @@ static void matroxfb_remove(WPMINFO int dummy) {
 	matroxfb_unregister_device(MINFO);
 	unregister_framebuffer(&ACCESS_FBINFO(fbcon));
 	matroxfb_g450_shutdown(PMINFO2);
-	del_timer_sync(&ACCESS_FBINFO(cursor.timer));
 #ifdef CONFIG_MTRR
 	if (ACCESS_FBINFO(mtrr.vram_valid))
 		mtrr_del(ACCESS_FBINFO(mtrr.vram), ACCESS_FBINFO(video.base), ACCESS_FBINFO(video.len));
@@ -235,7 +343,6 @@ static void matroxfb_remove(WPMINFO int dummy) {
 	release_mem_region(ACCESS_FBINFO(video.base), ACCESS_FBINFO(video.len_maximum));
 	release_mem_region(ACCESS_FBINFO(mmio.base), 16384);
 #ifdef CONFIG_FB_MATROX_MULTIHEAD
-	kfree(ACCESS_FBINFO(fbcon.disp));
 	kfree(minfo);
 #endif
 }
@@ -246,69 +353,51 @@ static void matroxfb_remove(WPMINFO int dummy) {
 
 static int matroxfb_open(struct fb_info *info, int user)
 {
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-	DBG_LOOP("matroxfb_open")
+	MINFO_FROM_INFO(info);
+	
+	DBG_LOOP(__FUNCTION__)
 
 	if (ACCESS_FBINFO(dead)) {
 		return -ENXIO;
 	}
 	ACCESS_FBINFO(usecount)++;
-#undef minfo
+	if (user) {
+		ACCESS_FBINFO(userusecount)++;
+	}
 	return(0);
 }
 
 static int matroxfb_release(struct fb_info *info, int user)
 {
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-	DBG_LOOP("matroxfb_release")
+	MINFO_FROM_INFO(info);
+	
+	DBG_LOOP(__FUNCTION__)
 
+	if (user) {
+		if (0 == --ACCESS_FBINFO(userusecount)) {
+			matroxfb_disable_irq(PMINFO2);
+		}
+	}
 	if (!(--ACCESS_FBINFO(usecount)) && ACCESS_FBINFO(dead)) {
 		matroxfb_remove(PMINFO 0);
 	}
-#undef minfo
 	return(0);
 }
 
-static int matroxfb_pan_display(struct fb_var_screeninfo *var, int con,
+static int matroxfb_pan_display(struct fb_var_screeninfo *var,
 		struct fb_info* info) {
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-
-	DBG("matroxfb_pan_display")
+	MINFO_FROM_INFO(info);
 
-	if (var->vmode & FB_VMODE_YWRAP) {
-		if (var->yoffset < 0 || var->yoffset >= fb_display[con].var.yres_virtual || var->xoffset)
-			return -EINVAL;
-	} else {
-		if (var->xoffset+fb_display[con].var.xres > fb_display[con].var.xres_virtual ||
-		    var->yoffset+fb_display[con].var.yres > fb_display[con].var.yres_virtual)
-			return -EINVAL;
-	}
-	if (con == ACCESS_FBINFO(fbcon.currcon))
-		matrox_pan_var(PMINFO var);
-	fb_display[con].var.xoffset = var->xoffset;
-	fb_display[con].var.yoffset = var->yoffset;
-	if (var->vmode & FB_VMODE_YWRAP)
-		fb_display[con].var.vmode |= FB_VMODE_YWRAP;
-	else
-		fb_display[con].var.vmode &= ~FB_VMODE_YWRAP;
-	return 0;
-#undef minfo
-}
+	DBG(__FUNCTION__)
 
-static int matroxfb_updatevar(int con, struct fb_info *info)
-{
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-	DBG("matroxfb_updatevar");
-
-	matrox_pan_var(PMINFO &fb_display[con].var);
+	matrox_pan_var(PMINFO var);
 	return 0;
-#undef minfo
 }
 
 static int matroxfb_get_final_bppShift(CPMINFO int bpp) {
 	int bppshft2;
 
-	DBG("matroxfb_get_final_bppShift")
+	DBG(__FUNCTION__)
 
 	bppshft2 = bpp;
 	if (!bppshft2) {
@@ -325,7 +414,7 @@ static int matroxfb_test_and_set_rounding(CPMINFO int xres, int bpp) {
 	int over;
 	int rounding;
 
-	DBG("matroxfb_test_and_set_rounding")
+	DBG(__FUNCTION__)
 
 	switch (bpp) {
 		case 0:		return xres;
@@ -356,7 +445,7 @@ static int matroxfb_pitch_adjust(CPMINFO int xres, int bpp) {
 	const int* width;
 	int xres_new;
 
-	DBG("matroxfb_pitch_adjust")
+	DBG(__FUNCTION__)
 
 	if (!bpp) return xres;
 
@@ -382,41 +471,27 @@ static int matroxfb_pitch_adjust(CPMINFO int xres, int bpp) {
 
 static int matroxfb_get_cmap_len(struct fb_var_screeninfo *var) {
 
-	DBG("matroxfb_get_cmap_len")
+	DBG(__FUNCTION__)
 
 	switch (var->bits_per_pixel) {
-#ifdef FBCON_HAS_VGATEXT
-		case 0:
-			return 16;	/* pseudocolor... 16 entries HW palette */
-#endif
-#ifdef FBCON_HAS_CFB4
 		case 4:
 			return 16;	/* pseudocolor... 16 entries HW palette */
-#endif
-#ifdef FBCON_HAS_CFB8
 		case 8:
 			return 256;	/* pseudocolor... 256 entries HW palette */
-#endif
-#ifdef FBCON_HAS_CFB16
 		case 16:
 			return 16;	/* directcolor... 16 entries SW palette */
 					/* Mystique: truecolor, 16 entries SW palette, HW palette hardwired into 1:1 mapping */
-#endif
-#ifdef FBCON_HAS_CFB24
 		case 24:
 			return 16;	/* directcolor... 16 entries SW palette */
 					/* Mystique: truecolor, 16 entries SW palette, HW palette hardwired into 1:1 mapping */
-#endif
-#ifdef FBCON_HAS_CFB32
 		case 32:
 			return 16;	/* directcolor... 16 entries SW palette */
 					/* Mystique: truecolor, 16 entries SW palette, HW palette hardwired into 1:1 mapping */
-#endif
 	}
 	return 16;	/* return something reasonable... or panic()? */
 }
 
-static int matroxfb_decode_var(CPMINFO struct display* p, struct fb_var_screeninfo *var, int *visual, int *video_cmap_len, unsigned int* ydstorg) {
+static int matroxfb_decode_var(CPMINFO struct fb_var_screeninfo *var, int *visual, int *video_cmap_len, unsigned int* ydstorg) {
 	struct RGBT {
 		unsigned char bpp;
 		struct {
@@ -429,51 +504,26 @@ static int matroxfb_decode_var(CPMINFO struct display* p, struct fb_var_screenin
 		signed char visual;
 	};
 	static const struct RGBT table[]= {
-#if defined FBCON_HAS_VGATEXT
-		{ 0,{ 0,6},{0,6},{0,6},{ 0,0},MX_VISUAL_PSEUDOCOLOR},
-#endif
-#if defined FBCON_HAS_CFB4 || defined FBCON_HAS_CFB8
 		{ 8,{ 0,8},{0,8},{0,8},{ 0,0},MX_VISUAL_PSEUDOCOLOR},
-#endif
-#if defined FBCON_HAS_CFB16
 		{15,{10,5},{5,5},{0,5},{15,1},MX_VISUAL_DIRECTCOLOR},
 		{16,{11,5},{5,6},{0,5},{ 0,0},MX_VISUAL_DIRECTCOLOR},
-#endif
-#if defined FBCON_HAS_CFB24
 		{24,{16,8},{8,8},{0,8},{ 0,0},MX_VISUAL_DIRECTCOLOR},
-#endif
-#if defined FBCON_HAS_CFB32
 		{32,{16,8},{8,8},{0,8},{24,8},MX_VISUAL_DIRECTCOLOR}
-#endif
 	};
 	struct RGBT const *rgbt;
 	unsigned int bpp = var->bits_per_pixel;
 	unsigned int vramlen;
 	unsigned int memlen;
 
-	DBG("matroxfb_decode_var")
+	DBG(__FUNCTION__)
 
 	switch (bpp) {
-#ifdef FBCON_HAS_VGATEXT
-		case 0:	 if (!ACCESS_FBINFO(capable.text)) return -EINVAL;
-			 break;
-#endif
-#ifdef FBCON_HAS_CFB4
 		case 4:	 if (!ACCESS_FBINFO(capable.cfb4)) return -EINVAL;
 			 break;
-#endif
-#ifdef FBCON_HAS_CFB8
 		case 8:	 break;
-#endif
-#ifdef FBCON_HAS_CFB16
 		case 16: break;
-#endif
-#ifdef FBCON_HAS_CFB24
 		case 24: break;
-#endif
-#ifdef FBCON_HAS_CFB32
 		case 32: break;
-#endif
 		default: return -EINVAL;
 	}
 	*ydstorg = 0;
@@ -482,50 +532,39 @@ static int matroxfb_decode_var(CPMINFO struct display* p, struct fb_var_screenin
 		var->yres_virtual = var->yres;
 	if (var->xres_virtual < var->xres)
 		var->xres_virtual = var->xres;
-	if (bpp) {
-		var->xres_virtual = matroxfb_pitch_adjust(PMINFO var->xres_virtual, bpp);
+
+	var->xres_virtual = matroxfb_pitch_adjust(PMINFO var->xres_virtual, bpp);
+	memlen = var->xres_virtual * bpp * var->yres_virtual / 8;
+	if (memlen > vramlen) {
+		var->yres_virtual = vramlen * 8 / (var->xres_virtual * bpp);
 		memlen = var->xres_virtual * bpp * var->yres_virtual / 8;
-		if (memlen > vramlen) {
-			var->yres_virtual = vramlen * 8 / (var->xres_virtual * bpp);
-			memlen = var->xres_virtual * bpp * var->yres_virtual / 8;
-		}
-		/* There is hardware bug that no line can cross 4MB boundary */
-		/* give up for CFB24, it is impossible to easy workaround it */
-		/* for other try to do something */
-		if (!ACCESS_FBINFO(capable.cross4MB) && (memlen > 0x400000)) {
-			if (bpp == 24) {
-				/* sorry */
-			} else {
-				unsigned int linelen;
-				unsigned int m1 = linelen = var->xres_virtual * bpp / 8;
-				unsigned int m2 = PAGE_SIZE;	/* or 128 if you do not need PAGE ALIGNED address */
-				unsigned int max_yres;
-
-				while (m1) {
-					int t;
-
-					while (m2 >= m1) m2 -= m1;
-					t = m1;
-					m1 = m2;
-					m2 = t;
-				}
-				m2 = linelen * PAGE_SIZE / m2;
-				*ydstorg = m2 = 0x400000 % m2;
-				max_yres = (vramlen - m2) / linelen;
-				if (var->yres_virtual > max_yres)
-					var->yres_virtual = max_yres;
+	}
+	/* There is hardware bug that no line can cross 4MB boundary */
+	/* give up for CFB24, it is impossible to easy workaround it */
+	/* for other try to do something */
+	if (!ACCESS_FBINFO(capable.cross4MB) && (memlen > 0x400000)) {
+		if (bpp == 24) {
+			/* sorry */
+		} else {
+			unsigned int linelen;
+			unsigned int m1 = linelen = var->xres_virtual * bpp / 8;
+			unsigned int m2 = PAGE_SIZE;	/* or 128 if you do not need PAGE ALIGNED address */
+			unsigned int max_yres;
+
+			while (m1) {
+				int t;
+
+				while (m2 >= m1) m2 -= m1;
+				t = m1;
+				m1 = m2;
+				m2 = t;
 			}
+			m2 = linelen * PAGE_SIZE / m2;
+			*ydstorg = m2 = 0x400000 % m2;
+			max_yres = (vramlen - m2) / linelen;
+			if (var->yres_virtual > max_yres)
+				var->yres_virtual = max_yres;
 		}
-	} else {
-		matrox_text_round(PMINFO var, p);
-#if 0
-/* we must limit pixclock by mclk...
-   Millennium I:    66 MHz = 15000
-   Millennium II:   61 MHz = 16300
-   Millennium G200: 83 MHz = 12000 */
-		if (var->pixclock < 15000)
-			var->pixclock = 15000;	/* limit for "normal" gclk & mclk */
-#endif
 	}
 	/* YDSTLEN contains only signed 16bit value */
 	if (var->yres_virtual > 32767)
@@ -576,7 +615,7 @@ static int matroxfb_setcolreg(unsigned regno, unsigned red, unsigned green,
 	struct matrox_fb_info* minfo = container_of(fb_info, struct matrox_fb_info, fbcon);
 #endif
 
-	DBG("matroxfb_setcolreg")
+	DBG(__FUNCTION__)
 
 	/*
 	 *  Set a single color register. The values supplied are
@@ -604,61 +643,42 @@ static int matroxfb_setcolreg(unsigned regno, unsigned red, unsigned green,
 	transp = CNVT_TOHW(transp, ACCESS_FBINFO(fbcon).var.transp.length);
 
 	switch (ACCESS_FBINFO(fbcon).var.bits_per_pixel) {
-#if defined(FBCON_HAS_CFB8) || defined(FBCON_HAS_CFB4) || defined(FBCON_HAS_VGATEXT)
-#ifdef FBCON_HAS_VGATEXT
-	case 0:
-#endif
-#ifdef FBCON_HAS_CFB4
 	case 4:
-#endif
-#ifdef FBCON_HAS_CFB8
 	case 8:
-#endif
 		mga_outb(M_DAC_REG, regno);
 		mga_outb(M_DAC_VAL, red);
 		mga_outb(M_DAC_VAL, green);
 		mga_outb(M_DAC_VAL, blue);
 		break;
-#endif
-#ifdef FBCON_HAS_CFB16
 	case 16:
-		ACCESS_FBINFO(cmap.cfb16[regno]) =
-			(red << ACCESS_FBINFO(fbcon).var.red.offset)     |
-			(green << ACCESS_FBINFO(fbcon).var.green.offset) |
-			(blue << ACCESS_FBINFO(fbcon).var.blue.offset)   |
-			(transp << ACCESS_FBINFO(fbcon).var.transp.offset); /* for 1:5:5:5 */
+		{
+			u_int16_t col =
+				(red << ACCESS_FBINFO(fbcon).var.red.offset)     |
+				(green << ACCESS_FBINFO(fbcon).var.green.offset) |
+				(blue << ACCESS_FBINFO(fbcon).var.blue.offset)   |
+				(transp << ACCESS_FBINFO(fbcon).var.transp.offset); /* for 1:5:5:5 */
+			ACCESS_FBINFO(cmap[regno]) = col | (col << 16);
+		}
 		break;
-#endif
-#ifdef FBCON_HAS_CFB24
 	case 24:
-		ACCESS_FBINFO(cmap.cfb24[regno]) =
-			(red   << ACCESS_FBINFO(fbcon).var.red.offset)   |
-			(green << ACCESS_FBINFO(fbcon).var.green.offset) |
-			(blue  << ACCESS_FBINFO(fbcon).var.blue.offset);
-		break;
-#endif
-#ifdef FBCON_HAS_CFB32
 	case 32:
-		ACCESS_FBINFO(cmap.cfb32[regno]) =
+		ACCESS_FBINFO(cmap[regno]) =
 			(red   << ACCESS_FBINFO(fbcon).var.red.offset)   |
 			(green << ACCESS_FBINFO(fbcon).var.green.offset) |
 			(blue  << ACCESS_FBINFO(fbcon).var.blue.offset)  |
 			(transp << ACCESS_FBINFO(fbcon).var.transp.offset);	/* 8:8:8:8 */
 		break;
-#endif
 	}
 	return 0;
 }
 
-static void matroxfb_update_fix(WPMINFO2)
+static void matroxfb_init_fix(WPMINFO2)
 {
 	struct fb_fix_screeninfo *fix = &ACCESS_FBINFO(fbcon).fix;
-	DBG("matroxfb_get_fix")
+	DBG(__FUNCTION__)
 
 	strcpy(fix->id,"MATROX");
 
-	fix->smem_start = ACCESS_FBINFO(video.base) + ACCESS_FBINFO(curr.ydstorg.bytes);
-	fix->smem_len = ACCESS_FBINFO(video.len_usable) - ACCESS_FBINFO(curr.ydstorg.bytes);
 	fix->xpanstep = 8;	/* 8 for 8bpp, 4 for 16bpp, 2 for 32bpp */
 	fix->ypanstep = 1;
 	fix->ywrapstep = 0;
@@ -667,91 +687,66 @@ static void matroxfb_update_fix(WPMINFO2)
 	fix->accel = ACCESS_FBINFO(devflags.accelerator);
 }
 
-static int matroxfb_set_var(struct fb_var_screeninfo *var, int con,
-			 struct fb_info *info)
+static void matroxfb_update_fix(WPMINFO2)
+{
+	struct fb_fix_screeninfo *fix = &ACCESS_FBINFO(fbcon).fix;
+	DBG(__FUNCTION__)
+
+	fix->smem_start = ACCESS_FBINFO(video.base) + ACCESS_FBINFO(curr.ydstorg.bytes);
+	fix->smem_len = ACCESS_FBINFO(video.len_usable) - ACCESS_FBINFO(curr.ydstorg.bytes);
+}
+
+static int matroxfb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+	int err;
+	int visual;
+	int cmap_len;
+	unsigned int ydstorg;
+	MINFO_FROM_INFO(info);
+
+	if (ACCESS_FBINFO(dead)) {
+		return -ENXIO;
+	}
+	if ((err = matroxfb_decode_var(PMINFO var, &visual, &cmap_len, &ydstorg)) != 0)
+		return err;
+	return 0;
+}
+
+static int matroxfb_set_par(struct fb_info *info)
 {
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
 	int err;
 	int visual;
 	int cmap_len;
 	unsigned int ydstorg;
-	struct display* display;
-	int chgvar;
+	struct fb_var_screeninfo *var;
+	MINFO_FROM_INFO(info);
 
-	DBG("matroxfb_set_var")
+	DBG(__FUNCTION__)
 
 	if (ACCESS_FBINFO(dead)) {
 		return -ENXIO;
 	}
 
-	if (con >= 0)
-		display = fb_display + con;
-	else
-		display = ACCESS_FBINFO(fbcon.disp);
-	if ((err = matroxfb_decode_var(PMINFO display, var, &visual, &cmap_len, &ydstorg)) != 0)
+	var = &info->var;
+	if ((err = matroxfb_decode_var(PMINFO var, &visual, &cmap_len, &ydstorg)) != 0)
 		return err;
-	switch (var->activate & FB_ACTIVATE_MASK) {
-		case FB_ACTIVATE_TEST:	return 0;
-		case FB_ACTIVATE_NXTOPEN:	/* ?? */
-		case FB_ACTIVATE_NOW:	break;	/* continue */
-		default:		return -EINVAL; /* unknown */
-	}
-	if (con >= 0) {
-		chgvar = ((display->var.xres != var->xres) ||
-		    (display->var.yres != var->yres) ||
-                    (display->var.xres_virtual != var->xres_virtual) ||
-		    (display->var.yres_virtual != var->yres_virtual) ||
-		    (display->var.bits_per_pixel != var->bits_per_pixel) ||
-		    memcmp(&display->var.red, &var->red, sizeof(var->red)) ||
-		    memcmp(&display->var.green, &var->green, sizeof(var->green)) ||
-		    memcmp(&display->var.blue, &var->blue, sizeof(var->blue)));
-	} else {
-		chgvar = 0;
-	}
-	display->var = *var;
-	/* cmap */
 	ACCESS_FBINFO(fbcon.screen_base) = vaddr_va(ACCESS_FBINFO(video.vbase)) + ydstorg;
-	if (display == ACCESS_FBINFO(currcon_display)) {
-		ACCESS_FBINFO(fbcon).var = *var;
-		matroxfb_update_fix(PMINFO2);
-		ACCESS_FBINFO(fbcon).fix.visual = visual;
-		if (var->bits_per_pixel) {
-			ACCESS_FBINFO(fbcon).fix.type = FB_TYPE_PACKED_PIXELS;
-			ACCESS_FBINFO(fbcon).fix.type_aux = 0;
-			display->next_line = ACCESS_FBINFO(fbcon).fix.line_length = (var->xres_virtual * var->bits_per_pixel) >> 3;
-		} else {
-			ACCESS_FBINFO(fbcon).fix.type = FB_TYPE_TEXT;
-			ACCESS_FBINFO(fbcon).fix.type_aux = ACCESS_FBINFO(devflags.text_type_aux);
-			display->next_line = ACCESS_FBINFO(fbcon).fix.line_length = (var->xres_virtual / (fontwidth(display)?fontwidth(display):8)) * ACCESS_FBINFO(devflags.textstep);
-		}
-	}
-	display->can_soft_blank = 1;
-	display->inverse = ACCESS_FBINFO(devflags.inverse);
-	/* conp, fb_info, vrows, cursor_x, cursor_y, fgcol, bgcol */
-	/* next_plane, fontdata, _font*, userfont */
-	initMatrox(PMINFO display);	/* dispsw */
-	/* dispsw, scrollmode, yscroll */
-	/* fgshift, bgshift, charmask */
-	if (chgvar && info && info->changevar)
-		info->changevar(con);
-	if (con == ACCESS_FBINFO(fbcon.currcon)) {
+	matroxfb_update_fix(PMINFO2);
+	ACCESS_FBINFO(fbcon).fix.visual = visual;
+	ACCESS_FBINFO(fbcon).fix.type = FB_TYPE_PACKED_PIXELS;
+	ACCESS_FBINFO(fbcon).fix.type_aux = 0;
+	ACCESS_FBINFO(fbcon).fix.line_length = (var->xres_virtual * var->bits_per_pixel) >> 3;
+	{
 		unsigned int pos;
 
 		ACCESS_FBINFO(curr.cmap_len) = cmap_len;
-		if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-			/* textmode must be in first megabyte, so no ydstorg allowed */
-			ACCESS_FBINFO(curr.ydstorg.bytes) = 0;
-			ACCESS_FBINFO(curr.ydstorg.chunks) = 0;
-			ACCESS_FBINFO(curr.ydstorg.pixels) = 0;
-		} else {
-			ydstorg += ACCESS_FBINFO(devflags.ydstorg);
-			ACCESS_FBINFO(curr.ydstorg.bytes) = ydstorg;
-			ACCESS_FBINFO(curr.ydstorg.chunks) = ydstorg >> (isInterleave(MINFO)?3:2);
-			if (var->bits_per_pixel == 4)
-				ACCESS_FBINFO(curr.ydstorg.pixels) = ydstorg;
-			else
-				ACCESS_FBINFO(curr.ydstorg.pixels) = (ydstorg * 8) / var->bits_per_pixel;
-		}
+		ydstorg += ACCESS_FBINFO(devflags.ydstorg);
+		ACCESS_FBINFO(curr.ydstorg.bytes) = ydstorg;
+		ACCESS_FBINFO(curr.ydstorg.chunks) = ydstorg >> (isInterleave(MINFO)?3:2);
+		if (var->bits_per_pixel == 4)
+			ACCESS_FBINFO(curr.ydstorg.pixels) = ydstorg;
+		else
+			ACCESS_FBINFO(curr.ydstorg.pixels) = (ydstorg * 8) / var->bits_per_pixel;
 		ACCESS_FBINFO(curr.final_bppShift) = matroxfb_get_final_bppShift(PMINFO var->bits_per_pixel);
 		if (visual == MX_VISUAL_PSEUDOCOLOR) {
 			int i;
@@ -783,9 +778,6 @@ static int matroxfb_set_var(struct fb_var_screeninfo *var, int con,
 
 			hw = &ACCESS_FBINFO(hw);
 
-			del_timer_sync(&ACCESS_FBINFO(cursor.timer));
-			ACCESS_FBINFO(cursor.state) = CM_ERASE;
-
 			down_read(&ACCESS_FBINFO(altout).lock);
 			for (out = 0; out < MATROXFB_MAX_OUTPUTS; out++) {
 				if (ACCESS_FBINFO(outputs[out]).src == MATROXFB_SRC_CRTC1 &&
@@ -796,22 +788,16 @@ static int matroxfb_set_var(struct fb_var_screeninfo *var, int con,
 			up_read(&ACCESS_FBINFO(altout).lock);
 			ACCESS_FBINFO(crtc1).pixclock = mt.pixclock;
 			ACCESS_FBINFO(crtc1).mnp = mt.mnp;
-			ACCESS_FBINFO(hw_switch->init(PMINFO &mt, display));
-			if (ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT) {
-				if (fontheight(display))
-					pos = var->yoffset / fontheight(display) * display->next_line / ACCESS_FBINFO(devflags.textstep) + var->xoffset / (fontwidth(display)?fontwidth(display):8);
-				else
-					pos = 0;
-			} else {
-				pos = (var->yoffset * var->xres_virtual + var->xoffset) * ACCESS_FBINFO(curr.final_bppShift) / 32;
-				pos += ACCESS_FBINFO(curr.ydstorg.chunks);
-			}
+			ACCESS_FBINFO(hw_switch->init(PMINFO &mt));
+			pos = (var->yoffset * var->xres_virtual + var->xoffset) * ACCESS_FBINFO(curr.final_bppShift) / 32;
+			pos += ACCESS_FBINFO(curr.ydstorg.chunks);
 
 			hw->CRTC[0x0D] = pos & 0xFF;
 			hw->CRTC[0x0C] = (pos & 0xFF00) >> 8;
 			hw->CRTCEXT[0] = (hw->CRTCEXT[0] & 0xF0) | ((pos >> 16) & 0x0F) | ((pos >> 14) & 0x40);
 			hw->CRTCEXT[8] = pos >> 21;
-			ACCESS_FBINFO(hw_switch->restore(PMINFO display));
+			ACCESS_FBINFO(hw_switch->restore(PMINFO2));
+			update_crtc2(PMINFO pos);
 			down_read(&ACCESS_FBINFO(altout).lock);
 			for (out = 0; out < MATROXFB_MAX_OUTPUTS; out++) {
 				if (ACCESS_FBINFO(outputs[out]).src == MATROXFB_SRC_CRTC1 &&
@@ -819,7 +805,6 @@ static int matroxfb_set_var(struct fb_var_screeninfo *var, int con,
 					ACCESS_FBINFO(outputs[out]).output->program(ACCESS_FBINFO(outputs[out]).data);
 				}
 			}
-			ACCESS_FBINFO(cursor.redraw) = 1;
 			for (out = 0; out < MATROXFB_MAX_OUTPUTS; out++) {
 				if (ACCESS_FBINFO(outputs[out]).src == MATROXFB_SRC_CRTC1 &&
 				    ACCESS_FBINFO(outputs[out]).output->start) {
@@ -828,94 +813,16 @@ static int matroxfb_set_var(struct fb_var_screeninfo *var, int con,
 			}
 			up_read(&ACCESS_FBINFO(altout).lock);
 			matrox_cfbX_init(PMINFO2);
-			my_install_cmap(PMINFO2);
 		}
 	}
 	return 0;
-#undef minfo
 }
 
-static int matrox_getcolreg(unsigned regno, unsigned *red, unsigned *green,
-			    unsigned *blue, unsigned *transp,
-			    struct fb_info *info)
-{
-
-	DBG("matrox_getcolreg")
-
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-	/*
-	 *  Read a single color register and split it into colors/transparent.
-	 *  Return != 0 for invalid regno.
-	 */
-
-	if (regno >= ACCESS_FBINFO(curr.cmap_len))
-		return 1;
-
-	*red   = ACCESS_FBINFO(palette[regno].red);
-	*green = ACCESS_FBINFO(palette[regno].green);
-	*blue  = ACCESS_FBINFO(palette[regno].blue);
-	*transp = ACCESS_FBINFO(palette[regno].transp);
-	return 0;
-#undef minfo
-}
-
-static int matroxfb_get_cmap(struct fb_cmap *cmap, int kspc, int con,
-			     struct fb_info *info)
-{
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-	struct display* dsp = (con < 0) ? ACCESS_FBINFO(fbcon.disp)
-					: fb_display + con;
-
-	DBG("matroxfb_get_cmap")
-
-	if (ACCESS_FBINFO(dead)) {
-		return -ENXIO;
-	}
-
-	if (con == ACCESS_FBINFO(fbcon.currcon)) /* current console? */
-		return fb_get_cmap(cmap, kspc, matrox_getcolreg, info);
-	else if (dsp->cmap.len) /* non default colormap? */
-		fb_copy_cmap(&dsp->cmap, cmap, kspc ? 0 : 2);
-	else
-		fb_copy_cmap(fb_default_cmap(matroxfb_get_cmap_len(&dsp->var)),
-			     cmap, kspc ? 0 : 2);
-	return 0;
-#undef minfo
-}
-
-static int matroxfb_set_cmap(struct fb_cmap *cmap, int kspc, int con,
-			     struct fb_info *info)
-{
-	unsigned int cmap_len;
-	struct display* dsp = (con < 0) ? info->disp : (fb_display + con);
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-
-	DBG("matroxfb_set_cmap")
-
-	if (ACCESS_FBINFO(dead)) {
-		return -ENXIO;
-	}
-
-	cmap_len = matroxfb_get_cmap_len(&dsp->var);
-	if (dsp->cmap.len != cmap_len) {
-		int err;
-
-		err = fb_alloc_cmap(&dsp->cmap, cmap_len, 0);
-		if (err)
-			return err;
-	}
-	if (con == ACCESS_FBINFO(fbcon.currcon)) {			/* current console? */
-		return fb_set_cmap(cmap, kspc, info);
-	} else
-		fb_copy_cmap(cmap, &dsp->cmap, kspc ? 0 : 1);
-	return 0;
-#undef minfo
-}
-
-static int matroxfb_get_vblank(CPMINFO struct fb_vblank *vblank)
+static int matroxfb_get_vblank(WPMINFO struct fb_vblank *vblank)
 {
 	unsigned int sts1;
 
+	matroxfb_enable_irq(PMINFO 0);
 	memset(vblank, 0, sizeof(*vblank));
 	vblank->flags = FB_VBLANK_HAVE_VCOUNT | FB_VBLANK_HAVE_VSYNC |
 			FB_VBLANK_HAVE_VBLANK | FB_VBLANK_HAVE_HBLANK;
@@ -930,8 +837,12 @@ static int matroxfb_get_vblank(CPMINFO struct fb_vblank *vblank)
 		vblank->flags |= FB_VBLANK_VSYNCING;
 	if (vblank->vcount >= ACCESS_FBINFO(fbcon).var.yres)
 		vblank->flags |= FB_VBLANK_VBLANKING;
-	vblank->hcount = 0;
-	vblank->count = 0;
+	if (test_bit(0, &ACCESS_FBINFO(irq_flags))) {
+		vblank->flags |= FB_VBLANK_HAVE_COUNT;
+		/* Only one writer, aligned int value... 
+		   it should work without lock and without atomic_t */
+		vblank->count = ACCESS_FBINFO(crtc1).vsync.cnt;
+	}
 	return 0;
 }
 
@@ -940,11 +851,12 @@ static struct matrox_altout panellink_output = {
 };
 
 static int matroxfb_ioctl(struct inode *inode, struct file *file,
-			  unsigned int cmd, unsigned long arg, int con,
+			  unsigned int cmd, unsigned long arg,
 			  struct fb_info *info)
 {
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-	DBG("matroxfb_ioctl")
+	MINFO_FROM_INFO(info);
+	
+	DBG(__FUNCTION__)
 
 	if (ACCESS_FBINFO(dead)) {
 		return -ENXIO;
@@ -963,6 +875,15 @@ static int matroxfb_ioctl(struct inode *inode, struct file *file,
 					return -EFAULT;
 				return 0;
 			}
+		case FBIO_WAITFORVSYNC:
+			{
+				u_int32_t crt;
+
+				if (get_user(crt, (u_int32_t *)arg))
+					return -EFAULT;
+
+				return matroxfb_wait_for_sync(PMINFO crt);
+			}
 		case MATROXFB_SET_OUTPUT_MODE:
 			{
 				struct matroxioc_output_mode mom;
@@ -997,7 +918,7 @@ static int matroxfb_ioctl(struct inode *inode, struct file *file,
 					return val;
 				switch (ACCESS_FBINFO(outputs[mom.output]).src) {
 					case MATROXFB_SRC_CRTC1:
-						matroxfb_switch(ACCESS_FBINFO(fbcon.currcon), info);
+						matroxfb_set_par(info);
 						break;
 					case MATROXFB_SRC_CRTC2:
 						{
@@ -1006,7 +927,7 @@ static int matroxfb_ioctl(struct inode *inode, struct file *file,
 							down_read(&ACCESS_FBINFO(crtc2.lock));
 							crtc2 = ACCESS_FBINFO(crtc2.info);
 							if (crtc2)
-								crtc2->fbcon.switch_con(crtc2->fbcon.currcon, &crtc2->fbcon);
+								crtc2->fbcon.fbops->fb_set_par(&crtc2->fbcon);
 							up_read(&ACCESS_FBINFO(crtc2.lock));
 						}
 						break;
@@ -1086,7 +1007,7 @@ static int matroxfb_ioctl(struct inode *inode, struct file *file,
 				}
 				if (!changes)
 					return 0;
-				matroxfb_switch(ACCESS_FBINFO(fbcon.currcon), info);
+				matroxfb_set_par(info);
 				return 0;
 			}
 		case MATROXFB_GET_OUTPUT_CONNECTION:
@@ -1142,21 +1063,98 @@ static int matroxfb_ioctl(struct inode *inode, struct file *file,
 					return -EFAULT;
 				return 0;
 			}
+		case VIDIOC_QUERYCAP:
+			{
+				struct v4l2_capability r;
+				
+				memset(&r, 0, sizeof(r));
+				strcpy(r.driver, "matroxfb");
+				strcpy(r.card, "Matrox");
+				sprintf(r.bus_info, "PCI:%s", ACCESS_FBINFO(pcidev)->slot_name);
+				r.version = KERNEL_VERSION(1,0,0);
+				r.capabilities = V4L2_CAP_VIDEO_OUTPUT;
+				if (copy_to_user((void*)arg, &r, sizeof(r)))
+					return -EFAULT;
+				return 0;
+				
+			}
+		case VIDIOC_QUERYCTRL:
+			{
+				struct v4l2_queryctrl qctrl;
+				int err;
+
+				if (copy_from_user(&qctrl, (struct v4l2_queryctrl*)arg, sizeof(qctrl)))
+					return -EFAULT;
+
+				down_read(&ACCESS_FBINFO(altout).lock);
+				if (!ACCESS_FBINFO(outputs[1]).output) {
+					err = -ENXIO;
+				} else if (ACCESS_FBINFO(outputs[1]).output->getqueryctrl) {
+					err = ACCESS_FBINFO(outputs[1]).output->getqueryctrl(ACCESS_FBINFO(outputs[1]).data, &qctrl);
+				} else {
+					err = -EINVAL;
+				}
+				up_read(&ACCESS_FBINFO(altout).lock);
+				if (err >= 0 &&
+				    copy_to_user((struct v4l2_queryctrl*)arg, &qctrl, sizeof(qctrl)))
+					return -EFAULT;
+				return err;
+			}
+		case VIDIOC_G_CTRL:
+			{
+				struct v4l2_control ctrl;
+				int err;
+
+				if (copy_from_user(&ctrl, (struct v4l2_control*)arg, sizeof(ctrl)))
+					return -EFAULT;
+
+				down_read(&ACCESS_FBINFO(altout).lock);
+				if (!ACCESS_FBINFO(outputs[1]).output) {
+					err = -ENXIO;
+				} else if (ACCESS_FBINFO(outputs[1]).output->getctrl) {
+					err = ACCESS_FBINFO(outputs[1]).output->getctrl(ACCESS_FBINFO(outputs[1]).data, &ctrl);
+				} else {
+					err = -EINVAL;
+				}
+				up_read(&ACCESS_FBINFO(altout).lock);
+				if (err >= 0 &&
+				    copy_to_user((struct v4l2_control*)arg, &ctrl, sizeof(ctrl)))
+					return -EFAULT;
+				return err;
+			}
+		case VIDIOC_S_CTRL:
+			{
+				struct v4l2_control ctrl;
+				int err;
+
+				if (copy_from_user(&ctrl, (struct v4l2_control*)arg, sizeof(ctrl)))
+					return -EFAULT;
+
+				down_read(&ACCESS_FBINFO(altout).lock);
+				if (!ACCESS_FBINFO(outputs[1]).output) {
+					err = -ENXIO;
+				} else if (ACCESS_FBINFO(outputs[1]).output->setctrl) {
+					err = ACCESS_FBINFO(outputs[1]).output->setctrl(ACCESS_FBINFO(outputs[1]).data, &ctrl);
+				} else {
+					err = -EINVAL;
+				}
+				up_read(&ACCESS_FBINFO(altout).lock);
+				return err;
+			}
 	}
 	return -ENOTTY;
-#undef minfo
 }
 
 /* 0 unblank, 1 blank, 2 no vsync, 3 no hsync, 4 off */
 
 static int matroxfb_blank(int blank, struct fb_info *info)
 {
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
 	int seq;
 	int crtc;
 	CRITFLAGS
+	MINFO_FROM_INFO(info);
 
-	DBG("matroxfb_blank")
+	DBG(__FUNCTION__)
 
 	if (ACCESS_FBINFO(dead))
 		return 1;
@@ -1178,72 +1176,24 @@ static int matroxfb_blank(int blank, struct fb_info *info)
 
 	CRITEND
 	return 0;
-#undef minfo
 }
 
 static struct fb_ops matroxfb_ops = {
 	.owner =	THIS_MODULE,
 	.fb_open =	matroxfb_open,
 	.fb_release =	matroxfb_release,
-	.fb_set_var =	matroxfb_set_var,
-	.fb_get_cmap =	matroxfb_get_cmap,
-	.fb_set_cmap =	matroxfb_set_cmap,
+	.fb_check_var =	matroxfb_check_var,
+	.fb_set_par =	matroxfb_set_par,
 	.fb_setcolreg =	matroxfb_setcolreg,
 	.fb_pan_display =matroxfb_pan_display,
 	.fb_blank =	matroxfb_blank,
 	.fb_ioctl =	matroxfb_ioctl,
+/*	.fb_fillrect =	<set by matrox_cfbX_init>, */
+/*	.fb_copyarea =	<set by matrox_cfbX_init>, */
+/*	.fb_imageblit =	<set by matrox_cfbX_init>, */
+/*	.fb_cursor =	<set by matrox_cfbX_init>, */
 };
 
-int matroxfb_switch(int con, struct fb_info *info)
-{
-#define minfo (container_of(info, struct matrox_fb_info, fbcon))
-	struct fb_cmap* cmap;
-	struct display *p;
-
-	DBG("matroxfb_switch");
-
-	if (ACCESS_FBINFO(fbcon.currcon) >= 0) {
-		/* Do we have to save the colormap? */
-		cmap = &(ACCESS_FBINFO(currcon_display)->cmap);
-		dprintk(KERN_DEBUG "switch1: con = %d, cmap.len = %d\n", ACCESS_FBINFO(fbcon.currcon), cmap->len);
-
-		if (cmap->len) {
-			dprintk(KERN_DEBUG "switch1a: %p %p %p %p\n", cmap->red, cmap->green, cmap->blue, cmap->transp);
-			fb_get_cmap(cmap, 1, matrox_getcolreg, info);
-#ifdef DEBUG
-			if (cmap->red) {
-				dprintk(KERN_DEBUG "switch1r: %X\n", cmap->red[0]);
-			}
-#endif
-		}
-	}
-	ACCESS_FBINFO(fbcon.currcon) = con;
-	if (con < 0)
-		p = ACCESS_FBINFO(fbcon.disp);
-	else
-		p = fb_display + con;
-	ACCESS_FBINFO(currcon_display) = p;
-	p->var.activate = FB_ACTIVATE_NOW;
-#ifdef DEBUG
-	cmap = &p->cmap;
-	dprintk(KERN_DEBUG "switch2: con = %d, cmap.len = %d\n", con, cmap->len);
-	dprintk(KERN_DEBUG "switch2a: %p %p %p %p\n", cmap->red, cmap->green, cmap->blue, cmap->transp);
-	if (p->cmap.red) {
-		dprintk(KERN_DEBUG "switch2r: %X\n", cmap->red[0]);
-	}
-#endif
-	matroxfb_set_var(&p->var, con, info);
-#ifdef DEBUG
-	dprintk(KERN_DEBUG "switch3: con = %d, cmap.len = %d\n", con, cmap->len);
-	dprintk(KERN_DEBUG "switch3a: %p %p %p %p\n", cmap->red, cmap->green, cmap->blue, cmap->transp);
-	if (p->cmap.red) {
-		dprintk(KERN_DEBUG "switch3r: %X\n", cmap->red[0]);
-	}
-#endif
-	return 0;
-#undef minfo
-}
-
 #define RSDepth(X)	(((X) >> 8) & 0x0F)
 #define RS8bpp		0x1
 #define RS15bpp		0x2
@@ -1278,14 +1228,11 @@ static int novga;			/* "matrox:novga" */
 static int nobios;			/* "matrox:nobios" */
 static int noinit = 1;			/* "matrox:init" */
 static int inverse;			/* "matrox:inverse" */
-static int hwcursor = 1;		/* "matrox:nohwcursor" */
-static int blink = 1;			/* "matrox:noblink" */
 static int sgram;			/* "matrox:sgram" */
 #ifdef CONFIG_MTRR
 static int mtrr = 1;			/* "matrox:nomtrr" */
 #endif
 static int grayscale;			/* "matrox:grayscale" */
-static unsigned int fastfont;		/* "matrox:fastfont:xxxxx" */
 static int dev = -1;			/* "matrox:dev:xxxxx" */
 static unsigned int vesa = ~0;		/* "matrox:vesa:xxxxx" */
 static int depth = -1;			/* "matrox:depth:xxxxx" */
@@ -1305,7 +1252,6 @@ static unsigned int maxclk;		/* "matrox:maxclk:xxxxM" */
 static int dfp;				/* "matrox:dfp */
 static int dfp_type = -1;		/* "matrox:dfp:xxx */
 static int memtype = -1;		/* "matrox:memtype:xxx" */
-static char fontname[64];		/* "matrox:font:xxxxx" */
 
 #ifndef MODULE
 static char videomode[64];		/* "matrox:mode:xxxxx" or "matrox:xxxxx" */
@@ -1319,7 +1265,7 @@ static int matroxfb_getmemory(WPMINFO unsigned int maxSize, unsigned int *realSi
 	unsigned char bytes[32];
 	unsigned char* tmp;
 
-	DBG("matroxfb_getmemory")
+	DBG(__FUNCTION__)
 
 	vm = ACCESS_FBINFO(video.vbase);
 	maxSize &= ~0x1FFFFF;	/* must be X*2MB (really it must be 2 or X*4MB) */
@@ -1571,13 +1517,13 @@ static struct fb_videomode defaultmode = {
 
 static int hotplug = 0;
 
-static int initMatrox2(WPMINFO struct display* d, struct board* b){
+static int initMatrox2(WPMINFO struct board* b){
 	unsigned long ctrlptr_phys = 0;
 	unsigned long video_base_phys = 0;
 	unsigned int memsize;
 	int err;
 
-	DBG("initMatrox2")
+	DBG(__FUNCTION__)
 
 	/* set default values... */
 	vesafb_defined.accel_flags = FB_ACCELF_TEXT;
@@ -1713,7 +1659,6 @@ static int initMatrox2(WPMINFO struct display* d, struct board* b){
 	ACCESS_FBINFO(devflags.ydstorg) = 0;
 
 	ACCESS_FBINFO(fbcon.currcon) = -1;
-	ACCESS_FBINFO(currcon_display) = d;
 	ACCESS_FBINFO(video.base) = video_base_phys;
 	ACCESS_FBINFO(video.len_usable) = ACCESS_FBINFO(video.len);
 	if (ACCESS_FBINFO(video.len_usable) > b->base->maxdisplayable)
@@ -1738,9 +1683,6 @@ static int initMatrox2(WPMINFO struct display* d, struct board* b){
 	ACCESS_FBINFO(fbcon.monspecs.dpms) = 0;	/* TBD */
 
 	/* static settings */
-	if ((depth == RSText8) && (!*ACCESS_FBINFO(fbcon.fontname))) {
-		strcpy(ACCESS_FBINFO(fbcon.fontname), "VGA8x8");
-	}
 	vesafb_defined.red = colors[depth-1].red;
 	vesafb_defined.green = colors[depth-1].green;
 	vesafb_defined.blue = colors[depth-1].blue;
@@ -1750,12 +1692,9 @@ static int initMatrox2(WPMINFO struct display* d, struct board* b){
 	if (noaccel)
 		vesafb_defined.accel_flags &= ~FB_ACCELF_TEXT;
 
-	strcpy(ACCESS_FBINFO(fbcon.modename), "MATROX VGA");
-	ACCESS_FBINFO(fbcon.changevar) = NULL;
-	ACCESS_FBINFO(fbcon.fbops) = &matroxfb_ops;
-	ACCESS_FBINFO(fbcon.disp) = d;
-	ACCESS_FBINFO(fbcon.switch_con) = &matroxfb_switch;
-	ACCESS_FBINFO(fbcon.updatevar) = &matroxfb_updatevar;
+	ACCESS_FBINFO(fbops) = matroxfb_ops;
+	ACCESS_FBINFO(fbcon.fbops) = &ACCESS_FBINFO(fbops);
+	ACCESS_FBINFO(fbcon.pseudo_palette) = ACCESS_FBINFO(cmap);
 	/* after __init time we are like module... no logo */
 	ACCESS_FBINFO(fbcon.flags) = hotplug ? FBINFO_FLAG_MODULE : FBINFO_FLAG_DEFAULT;
 	ACCESS_FBINFO(video.len_usable) &= PAGE_MASK;
@@ -1852,11 +1791,8 @@ static int initMatrox2(WPMINFO struct display* d, struct board* b){
 		vesafb_defined.yres_virtual = 65536; /* large enough to be INF, but small enough
 							to yres_virtual * xres_virtual < 2^32 */
 	}
+	matroxfb_init_fix(PMINFO2);
 	err = -EINVAL;
-	if (matroxfb_set_var(&vesafb_defined, -2, &ACCESS_FBINFO(fbcon))) {
-		printk(KERN_ERR "matroxfb: cannot set required parameters\n");
-		goto failVideoIO;
-	}
 
 	printk(KERN_INFO "matroxfb: %dx%dx%dbpp (virtual: %dx%d)\n",
 		vesafb_defined.xres, vesafb_defined.yres, vesafb_defined.bits_per_pixel,
@@ -1871,13 +1807,13 @@ static int initMatrox2(WPMINFO struct display* d, struct board* b){
 		goto failVideoIO;
 	}
 	printk("fb%d: %s frame buffer device\n",
-	       ACCESS_FBINFO(fbcon.node), ACCESS_FBINFO(fbcon.modename));
+	       ACCESS_FBINFO(fbcon.node), ACCESS_FBINFO(fbcon.fix.id));
 	if (ACCESS_FBINFO(fbcon.currcon) < 0) {
 		/* there is no console on this fb... but we have to initialize hardware
 		 * until someone tells me what is proper thing to do */
 		printk(KERN_INFO "fb%d: initializing hardware\n",
 			ACCESS_FBINFO(fbcon.node));
-		matroxfb_set_var(&vesafb_defined, -1, &ACCESS_FBINFO(fbcon));
+		fb_set_var(&ACCESS_FBINFO(fbcon), &vesafb_defined);
 	}
 	return 0;
 failVideoIO:;
@@ -1977,14 +1913,12 @@ static int matroxfb_probe(struct pci_dev* pdev, const struct pci_device_id* dumm
 	u_int16_t svid;
 	u_int16_t sid;
 	struct matrox_fb_info* minfo;
-	struct display* d;
 	int err;
 	u_int32_t cmd;
 #ifndef CONFIG_FB_MATROX_MULTIHEAD
 	static int registered = 0;
-	static struct display global_disp;
 #endif
-	DBG("matroxfb_probe")
+	DBG(__FUNCTION__)
 
 	pci_read_config_byte(pdev, PCI_REVISION_ID, &rev);
 	svid = pdev->subsystem_vendor;
@@ -2012,28 +1946,20 @@ static int matroxfb_probe(struct pci_dev* pdev, const struct pci_device_id* dumm
 	minfo = (struct matrox_fb_info*)kmalloc(sizeof(*minfo), GFP_KERNEL);
 	if (!minfo)
 		return -1;
-	d = (struct display*)kmalloc(sizeof(*d), GFP_KERNEL);
-	if (!d) {
-		kfree(minfo);
-		return -1;
-	}
 #else
 	if (registered)	/* singlehead driver... */
 		return -1;
 	minfo = &matroxfb_global_mxinfo;
-	d = &global_disp;
 #endif
 	memset(MINFO, 0, sizeof(*MINFO));
-	memset(d, 0, sizeof(*d));
 
 	ACCESS_FBINFO(pcidev) = pdev;
 	ACCESS_FBINFO(dead) = 0;
 	ACCESS_FBINFO(usecount) = 0;
+	ACCESS_FBINFO(userusecount) = 0;
+
 	pci_set_drvdata(pdev, MINFO);
-	/* CMDLINE */
-	memcpy(ACCESS_FBINFO(fbcon.fontname), fontname, sizeof(ACCESS_FBINFO(fbcon.fontname)));
 	/* DEVFLAGS */
-	ACCESS_FBINFO(devflags.inverse) = inverse;
 	ACCESS_FBINFO(devflags.memtype) = memtype;
 	if (memtype != -1)
 		noinit = 0;
@@ -2054,22 +1980,18 @@ static int matroxfb_probe(struct pci_dev* pdev, const struct pci_device_id* dumm
 	ACCESS_FBINFO(devflags.nopciretry) = no_pci_retry;
 	ACCESS_FBINFO(devflags.mga_24bpp_fix) = inv24;
 	ACCESS_FBINFO(devflags.precise_width) = option_precise_width;
-	ACCESS_FBINFO(devflags.hwcursor) = hwcursor;
-	ACCESS_FBINFO(devflags.blink) = blink;
 	ACCESS_FBINFO(devflags.sgram) = sgram;
 	ACCESS_FBINFO(capable.cross4MB) = cross4MB;
 
-	ACCESS_FBINFO(fastfont.size) = fastfont;
-
-	ACCESS_FBINFO(cursor.state) = CM_ERASE;
-	init_timer (&ACCESS_FBINFO(cursor.timer));
-	ACCESS_FBINFO(cursor.timer.data) = (unsigned long)MINFO;
 	spin_lock_init(&ACCESS_FBINFO(lock.DAC));
 	spin_lock_init(&ACCESS_FBINFO(lock.accel));
 	init_rwsem(&ACCESS_FBINFO(crtc2.lock));
 	init_rwsem(&ACCESS_FBINFO(altout.lock));
+	ACCESS_FBINFO(irq_flags) = 0;
+	init_waitqueue_head(&ACCESS_FBINFO(crtc1.vsync.wait));
+	init_waitqueue_head(&ACCESS_FBINFO(crtc2.vsync.wait));
 
-	err = initMatrox2(PMINFO d, b);
+	err = initMatrox2(PMINFO b);
 	if (!err) {
 #ifndef CONFIG_FB_MATROX_MULTIHEAD
 		registered = 1;
@@ -2078,7 +2000,6 @@ static int matroxfb_probe(struct pci_dev* pdev, const struct pci_device_id* dumm
 		return 0;
 	}
 #ifdef CONFIG_FB_MATROX_MULTIHEAD
-	kfree(d);
 	kfree(minfo);
 #endif
 	return -1;
@@ -2172,7 +2093,6 @@ static struct { int xres, yres, left, right, upper, lower, hslen, vslen, vfreq;
 #define RSCreate(X,Y)	((X) | ((Y) << 8))
 static struct { unsigned int vesa; unsigned int info; } *RSptr, vesamap[] __initdata = {
 /* default must be first */
-#ifdef FBCON_HAS_CFB8
 	{    ~0, RSCreate(RSNoxNo,     RS8bpp ) },
 	{ 0x101, RSCreate(RS640x480,   RS8bpp ) },
 	{ 0x100, RSCreate(RS640x400,   RS8bpp ) },
@@ -2184,9 +2104,6 @@ static struct { unsigned int vesa; unsigned int info; } *RSptr, vesamap[] __init
 	{ 0x107, RSCreate(RS1280x1024, RS8bpp ) },
 	{ 0x198, RSCreate(RS1408x1056, RS8bpp ) },
 	{ 0x11C, RSCreate(RS1600x1200, RS8bpp ) },
-#endif
-#ifdef FBCON_HAS_CFB16
-	{    ~0, RSCreate(RSNoxNo,     RS15bpp) },
 	{ 0x110, RSCreate(RS640x480,   RS15bpp) },
 	{ 0x181, RSCreate(RS768x576,   RS15bpp) },
 	{ 0x113, RSCreate(RS800x600,   RS15bpp) },
@@ -2205,9 +2122,6 @@ static struct { unsigned int vesa; unsigned int info; } *RSptr, vesamap[] __init
 	{ 0x11A, RSCreate(RS1280x1024, RS16bpp) },
 	{ 0x19A, RSCreate(RS1408x1056, RS16bpp) },
 	{ 0x11E, RSCreate(RS1600x1200, RS16bpp) },
-#endif
-#ifdef FBCON_HAS_CFB24
-	{    ~0, RSCreate(RSNoxNo,     RS24bpp) },
 	{ 0x1B2, RSCreate(RS640x480,   RS24bpp) },
 	{ 0x184, RSCreate(RS768x576,   RS24bpp) },
 	{ 0x1B5, RSCreate(RS800x600,   RS24bpp) },
@@ -2217,9 +2131,6 @@ static struct { unsigned int vesa; unsigned int info; } *RSptr, vesamap[] __init
 	{ 0x1BB, RSCreate(RS1280x1024, RS24bpp) },
 	{ 0x19C, RSCreate(RS1408x1056, RS24bpp) },
 	{ 0x1BF, RSCreate(RS1600x1200, RS24bpp) },
-#endif
-#ifdef FBCON_HAS_CFB32
-	{    ~0, RSCreate(RSNoxNo,     RS32bpp) },
 	{ 0x112, RSCreate(RS640x480,   RS32bpp) },
 	{ 0x183, RSCreate(RS768x576,   RS32bpp) },
 	{ 0x115, RSCreate(RS800x600,   RS32bpp) },
@@ -2229,27 +2140,11 @@ static struct { unsigned int vesa; unsigned int info; } *RSptr, vesamap[] __init
 	{ 0x11B, RSCreate(RS1280x1024, RS32bpp) },
 	{ 0x19B, RSCreate(RS1408x1056, RS32bpp) },
 	{ 0x11F, RSCreate(RS1600x1200, RS32bpp) },
-#endif
-#ifdef FBCON_HAS_VGATEXT
-	{    ~0, RSCreate(RSNoxNo,     RSText ) },
-	{ 0x002, RSCreate(RS640x400,   RSText ) },	/* 80x25 */
-	{ 0x003, RSCreate(RS640x400,   RSText ) },	/* 80x25 */
-	{ 0x007, RSCreate(RS640x400,   RSText ) },	/* 80x25 */
-	{ 0x1C0, RSCreate(RS640x400,   RSText8) },	/* 80x50 */
-	{ 0x108, RSCreate(RS640x480,   RSText8) },	/* 80x60 */
-	{ 0x109, RSCreate(RS1056x400,  RSText ) },	/* 132x25 */
-	{ 0x10A, RSCreate(RS1056x344,  RSText8) },	/* 132x43 */
-	{ 0x10B, RSCreate(RS1056x400,  RSText8) },	/* 132x50 */
-	{ 0x10C, RSCreate(RS1056x480,  RSText8) },	/* 132x60 */
-#endif
-#ifdef FBCON_HAS_CFB4
-	{    ~0, RSCreate(RSNoxNo,     RS4bpp ) },
 	{ 0x010, RSCreate(RS640x350,   RS4bpp ) },
 	{ 0x012, RSCreate(RS640x480,   RS4bpp ) },
 	{ 0x102, RSCreate(RS800x600,   RS4bpp ) },
 	{ 0x104, RSCreate(RS1024x768,  RS4bpp ) },
 	{ 0x106, RSCreate(RS1280x1024, RS4bpp ) },
-#endif
 	{     0, 0				}};
 
 static void __init matroxfb_init_params(void) {
@@ -2315,9 +2210,7 @@ static void __exit matrox_done(void) {
 int __init matroxfb_setup(char *options) {
 	char *this_opt;
 
-	DBG("matroxfb_setup")
-
-	fontname[0] = '\0';
+	DBG(__FUNCTION__)
 
 	if (!options || !*options)
 		return 0;
@@ -2363,8 +2256,6 @@ int __init matroxfb_setup(char *options) {
 			sync = simple_strtoul(this_opt+5, NULL, 0);
 		else if (!strncmp(this_opt, "vesa:", 5))
 			vesa = simple_strtoul(this_opt+5, NULL, 0);
-		else if (!strncmp(this_opt, "font:", 5))
-			strlcpy(fontname, this_opt+5, sizeof(fontname));
 		else if (!strncmp(this_opt, "maxclk:", 7))
 			maxclk = simple_strtoul(this_opt+7, NULL, 0);
 		else if (!strncmp(this_opt, "fh:", 3))
@@ -2379,7 +2270,7 @@ int __init matroxfb_setup(char *options) {
 			dfp_type = simple_strtoul(this_opt+4, NULL, 0);
 			dfp = 1;
 		}	
-#ifdef CONFIG_PPC
+#ifdef CONFIG_ALL_PPC
 		else if (!strncmp(this_opt, "vmode:", 6)) {
 			unsigned int vmode = simple_strtoul(this_opt+6, NULL, 0);
 			if (vmode > 0 && vmode <= VMODE_MAX)
@@ -2402,10 +2293,6 @@ int __init matroxfb_setup(char *options) {
 			}
 		}
 #endif
-		else if (!strncmp(this_opt, "fastfont:", 9))
-			fastfont = simple_strtoul(this_opt+9, NULL, 0);
-		else if (!strcmp(this_opt, "nofastfont"))	/* fastfont:N and nofastfont (nofastfont = fastfont:0) */
-			fastfont = 0;
 		else if (!strcmp(this_opt, "disabled"))	/* nodisabled does not exist */
 			disabled = 1;
 		else if (!strcmp(this_opt, "enabled"))	/* noenabled does not exist */
@@ -2445,10 +2332,6 @@ int __init matroxfb_setup(char *options) {
 				inv24 = value;
 			else if (!strcmp(this_opt, "cross4MB"))
 				cross4MB = value;
-			else if (!strcmp(this_opt, "hwcursor"))
-				hwcursor = value;
-			else if (!strcmp(this_opt, "blink"))
-				blink = value;
 			else if (!strcmp(this_opt, "grayscale"))
 				grayscale = value;
 			else if (!strcmp(this_opt, "dfp"))
@@ -2465,7 +2348,7 @@ static int __initdata initialized = 0;
 
 int __init matroxfb_init(void)
 {
-	DBG("matroxfb_init")
+	DBG(__FUNCTION__)
 
 	if (disabled)
 		return -ENXIO;
@@ -2550,12 +2433,6 @@ MODULE_PARM_DESC(fh, "Startup horizontal frequency, 0-999kHz, 1000-INF Hz");
 MODULE_PARM(fv, "i");
 MODULE_PARM_DESC(fv, "Startup vertical frequency, 0-INF Hz\n"
 "You should specify \"fv:max_monitor_vsync,fh:max_monitor_hsync,maxclk:max_monitor_dotclock\"\n");
-MODULE_PARM(hwcursor, "i");
-MODULE_PARM_DESC(hwcursor, "Enables hardware cursor (0 or 1) (default=0)");
-MODULE_PARM(blink, "i");
-MODULE_PARM_DESC(blink, "Enables hardware cursor blinking (0 or 1) (default=1)");
-MODULE_PARM(fastfont, "i");
-MODULE_PARM_DESC(fastfont, "Specifies, how much memory should be used for font data (0, 1024-65536 are reasonable) (default=0)");
 MODULE_PARM(grayscale, "i");
 MODULE_PARM_DESC(grayscale, "Sets display into grayscale. Works perfectly with paletized videomode (4, 8bpp), some limitations apply to 16, 24 and 32bpp videomodes (default=nograyscale)");
 MODULE_PARM(cross4MB, "i");
@@ -2564,7 +2441,7 @@ MODULE_PARM(dfp, "i");
 MODULE_PARM_DESC(dfp, "Specifies whether to use digital flat panel interface of G200/G400 (0 or 1) (default=0)");
 MODULE_PARM(dfp_type, "i");
 MODULE_PARM_DESC(dfp_type, "Specifies DFP interface type (0 to 255) (default=read from hardware)");
-#ifdef CONFIG_PPC
+#ifdef CONFIG_ALL_PPC
 MODULE_PARM(vmode, "i");
 MODULE_PARM_DESC(vmode, "Specify the vmode mode number that should be used (640x480 default)");
 MODULE_PARM(cmode, "i");
@@ -2573,7 +2450,7 @@ MODULE_PARM_DESC(cmode, "Specify the video depth that should be used (8bit defau
 
 int __init init_module(void){
 
-	DBG("init_module")
+	DBG(__FUNCTION__)
 
 	if (disabled)
 		return -ENXIO;
@@ -2605,7 +2482,7 @@ int __init init_module(void){
 module_exit(matrox_done);
 EXPORT_SYMBOL(matroxfb_register_driver);
 EXPORT_SYMBOL(matroxfb_unregister_driver);
-EXPORT_SYMBOL(matroxfb_switch);
+EXPORT_SYMBOL(matroxfb_wait_for_sync);
 
 /*
  * Overrides for Emacs so that we follow Linus's tabbing style.
@@ -2614,3 +2491,4 @@ EXPORT_SYMBOL(matroxfb_switch);
  * c-basic-offset: 8
  * End:
  */
+

drivers/video/matrox/matroxfb_base.h

@@ -42,6 +42,7 @@
 #include <linux/timer.h>
 #include <linux/pci.h>
 #include <linux/spinlock.h>
+#include <linux/kd.h>
 
 #include <asm/io.h>
 #include <asm/unaligned.h>
@@ -49,12 +50,7 @@
 #include <asm/mtrr.h>
 #endif
 
-#include <video/fbcon.h>
-#include <video/fbcon-cfb4.h>
-#include <video/fbcon-cfb8.h>
-#include <video/fbcon-cfb16.h>
-#include <video/fbcon-cfb24.h>
-#include <video/fbcon-cfb32.h>
+#include "../console/fbcon.h"
 
 #if defined(CONFIG_PPC)
 #include <asm/prom.h>
@@ -65,8 +61,6 @@
 /* always compile support for 32MB... It cost almost nothing */
 #define CONFIG_FB_MATROX_32MB
 
-#define FBCON_HAS_VGATEXT
-
 #ifdef MATROXFB_DEBUG
 
 #define DEBUG
@@ -338,8 +332,6 @@ struct matroxfb_par
 		unsigned int pixels;
 		unsigned int chunks;
 		      } ydstorg;
-	void		(*putc)(u_int32_t, u_int32_t, struct display*, int, int, int);
-	void		(*putcs)(u_int32_t, u_int32_t, struct display*, const unsigned short*, int, int, int);
 };
 
 struct matrox_fb_info;
@@ -347,7 +339,6 @@ struct matrox_fb_info;
 struct matrox_DAC1064_features {
 	u_int8_t	xvrefctrl;
 	u_int8_t	xmiscctrl;
-	unsigned int	cursorimage;
 };
 
 struct matrox_accel_features {
@@ -398,12 +389,21 @@ struct matrox_accel_data {
 	u_int32_t	m_opmode;
 };
 
+struct v4l2_queryctrl;
+struct v4l2_control;
+
 struct matrox_altout {
 	const char	*name;
 	int		(*compute)(void* altout_dev, struct my_timming* input);
 	int		(*program)(void* altout_dev);
 	int		(*start)(void* altout_dev);
 	int		(*verifymode)(void* altout_dev, u_int32_t mode);
+	int		(*getqueryctrl)(void* altout_dev,
+					struct v4l2_queryctrl* ctrl);
+	int		(*getctrl)(void* altout_dev, 
+				   struct v4l2_control* ctrl);
+	int		(*setctrl)(void* altout_dev, 
+				   struct v4l2_control* ctrl);
 };
 
 #define MATROXFB_SRC_NONE	0
@@ -424,10 +424,17 @@ struct matrox_bios {
 		      } output;
 };
 
+extern struct display fb_display[];
+
 struct matrox_switch;
 struct matroxfb_driver;
 struct matroxfb_dh_fb_info;
 
+struct matrox_vsync {
+	wait_queue_head_t	wait;
+	unsigned int		cnt;
+};
+
 struct matrox_fb_info {
 	struct fb_info		fbcon;
 
@@ -436,6 +443,9 @@ struct matrox_fb_info {
 	int			dead;
 	unsigned int		usecount;
 
+	unsigned int		userusecount;
+	unsigned long		irq_flags;
+
 	struct matroxfb_par	curr;
 	struct matrox_hw_state	hw;
 
@@ -444,17 +454,23 @@ struct matrox_fb_info {
 	struct pci_dev*		pcidev;
 
 	struct {
+		struct matrox_vsync	vsync;
 		unsigned int	pixclock;
 		int		mnp;
 			      } crtc1;
 	struct {
+		struct matrox_vsync	vsync;
 		unsigned int 	pixclock;
 		int		mnp;
 	struct matroxfb_dh_fb_info*	info;
 	struct rw_semaphore	lock;
 			      } crtc2;
 	struct {
- 	struct rw_semaphore	lock;
+	struct rw_semaphore	lock;
+	struct {
+		int brightness, contrast, saturation, hue, gamma;
+		int testout, deflicker;
+				} tvo_params;
 			      } altout;
 #define MATROXFB_MAX_OUTPUTS		3
 	struct {
@@ -486,7 +502,6 @@ struct matrox_fb_info {
 	unsigned int	max_pixel_clock;
 
 	struct matrox_switch*	hw_switch;
-	struct display*	currcon_display;
 
 	struct {
 		struct matrox_pll_features pll;
@@ -511,11 +526,6 @@ struct matrox_fb_info {
 		int		plnwt;
 		int		srcorg;
 			      } capable;
-	struct {
-		unsigned int	size;
-		unsigned int	mgabase;
-		vaddr_t		vbase;
-			      } fastfont;
 #ifdef CONFIG_MTRR
 	struct {
 		int		vram;
@@ -529,9 +539,6 @@ struct matrox_fb_info {
 		int		nobios;
 		int		nopciretry;
 		int		noinit;
-		int		inverse;
-		int		hwcursor;
-		int		blink;
 		int		sgram;
 #ifdef CONFIG_FB_MATROX_32MB
 		int		support32MB;
@@ -555,18 +562,7 @@ struct matrox_fb_info {
 		int		dualhead;
 		unsigned int	fbResource;
 			      } devflags;
-	struct display_switch	dispsw;
-	struct {
-		int		x;
-		int		y;
-		unsigned int	w;
-		unsigned int	u;
-		unsigned int	d;
-		unsigned int	type;
-		int		state;
-		int		redraw;
-		struct timer_list timer;
-			      } cursor;
+	struct fb_ops		fbops;
 	struct matrox_bios	bios;
 	struct {
 		struct matrox_pll_limits	pixel;
@@ -600,22 +596,11 @@ struct matrox_fb_info {
 				      } memory;
 			      } values;
 	struct { unsigned red, green, blue, transp; } palette[256];
-/* These ifdefs must be last! They differ for module & non-module compiles */
-#if defined(FBCON_HAS_CFB16) || defined(FBCON_HAS_CFB24) || defined(FBCON_HAS_CFB32)
-	union {
-#ifdef FBCON_HAS_CFB16
-		u_int16_t	cfb16[16];
-#endif
-#ifdef FBCON_HAS_CFB24
-		u_int32_t	cfb24[16];
-#endif
-#ifdef FBCON_HAS_CFB32
-		u_int32_t	cfb32[16];
-#endif
-	} cmap;
-#endif
+	u_int32_t cmap[17];
 };
 
+#define info2minfo(info) container_of(info, struct matrox_fb_info, fbcon)
+
 #ifdef CONFIG_FB_MATROX_MULTIHEAD
 #define ACCESS_FBINFO2(info, x) (info->x)
 #define ACCESS_FBINFO(x) ACCESS_FBINFO2(minfo,x)
@@ -629,14 +614,7 @@ struct matrox_fb_info {
 #define PMINFO2  minfo
 #define PMINFO   PMINFO2 ,
 
-static inline struct matrox_fb_info* mxinfo(const struct display* p) {
-	return container_of(p->fb_info, struct matrox_fb_info, fbcon);
-}
-
-#define PMXINFO(p)	   mxinfo(p),
 #define MINFO_FROM(x)	   struct matrox_fb_info* minfo = x
-#define MINFO_FROM_DISP(x) MINFO_FROM(mxinfo(x))
-
 #else
 
 extern struct matrox_fb_info matroxfb_global_mxinfo;
@@ -653,24 +631,17 @@ extern struct matrox_fb_info matroxfb_global_mxinfo;
 #define PMINFO2
 #define PMINFO
 
-#if 0
-static inline struct matrox_fb_info* mxinfo(const struct display* p) {
-	return &matroxfb_global_mxinfo;
-}
-#endif
-
-#define PMXINFO(p)
 #define MINFO_FROM(x)
-#define MINFO_FROM_DISP(x)
 
 #endif
 
+#define MINFO_FROM_INFO(x) MINFO_FROM(info2minfo(x))
+
 struct matrox_switch {
 	int	(*preinit)(WPMINFO2);
 	void	(*reset)(WPMINFO2);
-	int	(*init)(WPMINFO struct my_timming*, struct display*);
-	void	(*restore)(WPMINFO struct display*);
-	int	(*selhwcursor)(WPMINFO2);
+	int	(*init)(WPMINFO struct my_timming*);
+	void	(*restore)(WPMINFO2);
 };
 
 struct matroxfb_driver {
@@ -746,7 +717,7 @@ void matroxfb_unregister_driver(struct matroxfb_driver* drv);
 
 #define M_FIFOSTATUS	0x1E10
 #define M_STATUS	0x1E14
-
+#define M_ICLEAR	0x1E18
 #define M_IEN		0x1E1C
 
 #define M_VCOUNT	0x1E20
@@ -868,7 +839,8 @@ extern void matroxfb_DAC_out(CPMINFO int reg, int val);
 extern int matroxfb_DAC_in(CPMINFO int reg);
 extern struct list_head matroxfb_list;
 extern void matroxfb_var2my(struct fb_var_screeninfo* fvsi, struct my_timming* mt);
-extern int matroxfb_switch(int con, struct fb_info *);
+extern int matroxfb_wait_for_sync(WPMINFO u_int32_t crtc);
+extern int matroxfb_enable_irq(WPMINFO int reenable);
 
 #ifdef MATROXFB_USE_SPINLOCKS
 #define CRITBEGIN  spin_lock_irqsave(&ACCESS_FBINFO(lock.accel), critflags);

drivers/video/matrox/matroxfb_crtc2.c

@@ -26,21 +26,9 @@ MODULE_PARM_DESC(mem, "Memory size reserved for dualhead (default=8MB)");
 
 /* **************************************************** */
 
-static int matroxfb_dh_getcolreg(unsigned regno, unsigned *red, unsigned *green,
-		unsigned *blue, unsigned *transp, struct fb_info* info) {
-#define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
-	if (regno >= 16)
-		return 1;
-	*red = m2info->palette[regno].red;
-	*blue = m2info->palette[regno].blue;
-	*green = m2info->palette[regno].green;
-	*transp = m2info->palette[regno].transp;
-	return 0;
-#undef m2info
-}
-
 static int matroxfb_dh_setcolreg(unsigned regno, unsigned red, unsigned green,
 		unsigned blue, unsigned transp, struct fb_info* info) {
+	u_int32_t col;
 #define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
 
 	if (regno >= 16)
@@ -58,43 +46,23 @@ static int matroxfb_dh_setcolreg(unsigned regno, unsigned red, unsigned green,
 	blue = CNVT_TOHW(blue, m2info->fbcon.var.blue.length);
 	transp = CNVT_TOHW(transp, m2info->fbcon.var.transp.length);
 
+	col = (red << m2info->fbcon.var.red.offset)     |
+	      (green << m2info->fbcon.var.green.offset) |
+	      (blue << m2info->fbcon.var.blue.offset)   |
+	      (transp << m2info->fbcon.var.transp.offset);
+
 	switch (m2info->fbcon.var.bits_per_pixel) {
-#ifdef FBCON_HAS_CFB16
 		case 16:
-			m2info->cmap.cfb16[regno] =
-				(red << m2info->fbcon.var.red.offset)     |
-				(green << m2info->fbcon.var.green.offset) |
-				(blue << m2info->fbcon.var.blue.offset)   |
-				(transp << m2info->fbcon.var.transp.offset);
+			m2info->cmap[regno] = col | (col << 16);
 			break;
-#endif
-#ifdef FBCON_HAS_CFB32
 		case 32:
-			m2info->cmap.cfb32[regno] =
-				(red << m2info->fbcon.var.red.offset)     |
-				(green << m2info->fbcon.var.green.offset) |
-				(blue << m2info->fbcon.var.blue.offset)   |
-				(transp << m2info->fbcon.var.transp.offset);
+			m2info->cmap[regno] = col;
 			break;
-#endif
 	}
 	return 0;
 #undef m2info
 }
 
-static inline void my_install_cmap(struct matroxfb_dh_fb_info* m2info)
-{
-	/* Do not touch this code if you do not understand what it does! */
-	/* Never try to use do_install_cmap() instead. It is crap. */
-	struct fb_cmap* cmap = &m2info->currcon_display->cmap;
-	
-	if (cmap->len)
-		fb_set_cmap(cmap, 1, &m2info->fbcon);
-	else
-		fb_set_cmap(fb_default_cmap(16), 1, &m2info->fbcon);
-}
-
-
 static void matroxfb_dh_restore(struct matroxfb_dh_fb_info* m2info,
 		struct my_timming* mt,
 		int mode,
@@ -201,6 +169,7 @@ static void matroxfb_dh_disable(struct matroxfb_dh_fb_info* m2info) {
 
 static void matroxfb_dh_cfbX_init(struct matroxfb_dh_fb_info* m2info) {
 	/* no acceleration for secondary head... */
+	m2info->cmap[16] = 0xFFFFFFFF;
 }
 
 static void matroxfb_dh_pan_var(struct matroxfb_dh_fb_info* m2info,
@@ -234,14 +203,10 @@ static int matroxfb_dh_decode_var(struct matroxfb_dh_fb_info* m2info,
 	unsigned int vramlen;
 
 	switch (var->bits_per_pixel) {
-#ifdef FBCON_HAS_CFB16
 		case 16:	mask = 0x1F;
 				break;
-#endif
-#ifdef FBCON_HAS_CFB32
 		case 32:	mask = 0x0F;
 				break;
-#endif
 		default:	return -EINVAL;
 	}
 	vramlen = m2info->video.len_usable;
@@ -302,35 +267,20 @@ static int matroxfb_dh_decode_var(struct matroxfb_dh_fb_info* m2info,
 	return 0;
 }
 
-static void initMatroxDH(struct matroxfb_dh_fb_info* m2info, struct display* p) {
-	switch (m2info->fbcon.var.bits_per_pixel) {
-#ifdef FBCON_HAS_CFB16
-		case 16:
-			p->dispsw_data = m2info->cmap.cfb16;
-			p->dispsw = &fbcon_cfb16;
-			break;
-#endif
-#ifdef FBCON_HAS_CFB32
-		case 32:
-			p->dispsw_data = m2info->cmap.cfb32;
-			p->dispsw = &fbcon_cfb32;
-			break;
-#endif
-		default:
-			p->dispsw_data = NULL;
-			p->dispsw = &fbcon_dummy;
-			break;
-	}
-}
-
 static int matroxfb_dh_open(struct fb_info* info, int user) {
 #define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
 	MINFO_FROM(m2info->primary_dev);
 
 	if (MINFO) {
+		int err;
+
 		if (ACCESS_FBINFO(dead)) {
 			return -ENXIO;
 		}
+		err = ACCESS_FBINFO(fbops).fb_open(&ACCESS_FBINFO(fbcon), user);
+		if (err) {
+			return err;
+		}
 	}
 	return 0;
 #undef m2info
@@ -338,15 +288,17 @@ static int matroxfb_dh_open(struct fb_info* info, int user) {
 
 static int matroxfb_dh_release(struct fb_info* info, int user) {
 #define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
+	int err = 0;
 	MINFO_FROM(m2info->primary_dev);
 
 	if (MINFO) {
+		err = ACCESS_FBINFO(fbops).fb_release(&ACCESS_FBINFO(fbcon), user);
 	}
-	return 0;
+	return err;
 #undef m2info
 }
 
-static void matroxfb_dh_update_fix(struct matroxfb_dh_fb_info *m2info) {
+static void matroxfb_dh_init_fix(struct matroxfb_dh_fb_info *m2info) {
 	struct fb_fix_screeninfo *fix = &m2info->fbcon.fix;
 
 	strcpy(fix->id, "MATROX DH");
@@ -361,57 +313,36 @@ static void matroxfb_dh_update_fix(struct matroxfb_dh_fb_info *m2info) {
 	fix->accel = 0;		/* no accel... */
 }
 
-static int matroxfb_dh_set_var(struct fb_var_screeninfo* var, int con,
-		struct fb_info* info) {
+static int matroxfb_dh_check_var(struct fb_var_screeninfo* var, struct fb_info* info) {
+#define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
+	int visual;
+	int cmap_len;
+	int mode;
+
+	return matroxfb_dh_decode_var(m2info, var, &visual, &cmap_len, &mode);
+#undef m2info
+}
+
+static int matroxfb_dh_set_par(struct fb_info* info) {
 #define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
-	struct display* p;
-	int chgvar;
 	int visual;
 	int cmap_len;
 	int mode;
 	int err;
+	struct fb_var_screeninfo* var = &info->var;
 	MINFO_FROM(m2info->primary_dev);
 
-	if (con < 0)
-		p = m2info->fbcon.disp;
-	else
-		p = fb_display + con;
 	if ((err = matroxfb_dh_decode_var(m2info, var, &visual, &cmap_len, &mode)) != 0)
 		return err;
-	switch (var->activate & FB_ACTIVATE_MASK) {
-		case FB_ACTIVATE_TEST:	return 0;
-		case FB_ACTIVATE_NXTOPEN:
-		case FB_ACTIVATE_NOW:	break;
-		default:		return -EINVAL;
-	}
-	if (con >= 0) {
-		chgvar = (p->var.xres != var->xres) ||
-			(p->var.yres != var->yres) ||
-			(p->var.xres_virtual != var->xres_virtual) ||
-			(p->var.yres_virtual != var->yres_virtual) ||
-			(p->var.bits_per_pixel != var->bits_per_pixel) ||
-			memcmp(&p->var.red, &var->red, sizeof(var->red)) ||
-			memcmp(&p->var.green, &var->green, sizeof(var->green)) ||
-			memcmp(&p->var.blue, &var->blue, sizeof(var->blue));
-	} else
-		chgvar = 0;
-	p->var = *var;
 	/* cmap */
-	if (con == m2info->fbcon.currcon) {
+	{
 		m2info->fbcon.screen_base = vaddr_va(m2info->video.vbase);
-		m2info->fbcon.var = *var;
 		m2info->fbcon.fix.visual = visual;
 		m2info->fbcon.fix.type = FB_TYPE_PACKED_PIXELS;
 		m2info->fbcon.fix.type_aux = 0;
-		p->next_line = m2info->fbcon.fix.line_length = (var->xres_virtual * var->bits_per_pixel) >> 3;
-		matroxfb_dh_update_fix(m2info);
+		m2info->fbcon.fix.line_length = (var->xres_virtual * var->bits_per_pixel) >> 3;
 	}
-	p->can_soft_blank = 0;
-	p->inverse = 0;	/* TBD */
-	initMatroxDH(m2info, p);
-	if (chgvar && info && info->changevar)
-		info->changevar(con);
-	if (con == m2info->fbcon.currcon) {
+	{
 		struct my_timming mt;
 		unsigned int pos;
 		int out;
@@ -459,74 +390,22 @@ static int matroxfb_dh_set_var(struct fb_var_screeninfo* var, int con,
 		}
 		up_read(&ACCESS_FBINFO(altout).lock);
 		matroxfb_dh_cfbX_init(m2info);
-		my_install_cmap(m2info);
-	}
-	return 0;
-#undef m2info
-}
-
-static int matroxfb_dh_get_cmap(struct fb_cmap* cmap, int kspc, int con,
-		struct fb_info* info) {
-#define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
-	struct display* dsp;
-
-	if (con < 0)
-		dsp = m2info->fbcon.disp;
-	else
-		dsp = fb_display + con;
-	if (con == m2info->fbcon.currcon)
-		return fb_get_cmap(cmap, kspc, matroxfb_dh_getcolreg, info);
-	else if (dsp->cmap.len)
-		fb_copy_cmap(&dsp->cmap, cmap, kspc ? 0 : 2);
-	else
-		fb_copy_cmap(fb_default_cmap(16), cmap, kspc ? 0 : 2);
-	return 0;
-#undef m2info
-}
-
-static int matroxfb_dh_set_cmap(struct fb_cmap* cmap, int kspc, int con,
-		struct fb_info* info) {
-#define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
-	struct display* dsp;
-
-	if (con < 0)
-		dsp = m2info->fbcon.disp;
-	else
-		dsp = fb_display + con;
-	if (dsp->cmap.len != 16) {
-		int err;
-
-		err = fb_alloc_cmap(&dsp->cmap, 16, 0);
-		if (err)
-			return err;
 	}
-	if (con == m2info->fbcon.currcon)
-		return fb_set_cmap(cmap, kspc, info);
-	else
-		fb_copy_cmap(cmap, &dsp->cmap, kspc ? 0 : 1);
 	return 0;
 #undef m2info
 }
 
-static int matroxfb_dh_pan_display(struct fb_var_screeninfo* var, int con,
-		struct fb_info* info) {
+static int matroxfb_dh_pan_display(struct fb_var_screeninfo* var, struct fb_info* info) {
 #define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
-	if (var->xoffset + fb_display[con].var.xres > fb_display[con].var.xres_virtual ||
-	    var->yoffset + fb_display[con].var.yres > fb_display[con].var.yres_virtual)
-		return -EINVAL;
-	if (con == m2info->fbcon.currcon)
-		matroxfb_dh_pan_var(m2info, var);
-	fb_display[con].var.xoffset = var->xoffset;
-	fb_display[con].var.yoffset = var->yoffset;
+	matroxfb_dh_pan_var(m2info, var);
 	return 0;
 #undef m2info
 }
 
-static int matroxfb_dh_switch(int con, struct fb_info* info);
-
 static int matroxfb_dh_get_vblank(const struct matroxfb_dh_fb_info* m2info, struct fb_vblank* vblank) {
 	MINFO_FROM(m2info->primary_dev);
 
+	matroxfb_enable_irq(PMINFO 0);
 	memset(vblank, 0, sizeof(*vblank));
 	vblank->flags = FB_VBLANK_HAVE_VCOUNT | FB_VBLANK_HAVE_VBLANK;
 	/* mask out reserved bits + field number (odd/even) */
@@ -534,6 +413,12 @@ static int matroxfb_dh_get_vblank(const struct matroxfb_dh_fb_info* m2info, stru
 	/* compatibility stuff */
 	if (vblank->vcount >= m2info->fbcon.var.yres)
 		vblank->flags |= FB_VBLANK_VBLANKING;
+        if (test_bit(0, &ACCESS_FBINFO(irq_flags))) {
+                vblank->flags |= FB_VBLANK_HAVE_COUNT;
+                /* Only one writer, aligned int value...
+                   it should work without lock and without atomic_t */
+                vblank->count = ACCESS_FBINFO(crtc2).vsync.cnt;
+        }
 	return 0;
 }
 
@@ -541,12 +426,11 @@ static int matroxfb_dh_ioctl(struct inode* inode,
 		struct file* file,
 		unsigned int cmd,
 		unsigned long arg,
-		int con,
 		struct fb_info* info) {
 #define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
 	MINFO_FROM(m2info->primary_dev);
 
-	DBG("matroxfb_crtc2_ioctl")
+	DBG(__FUNCTION__)
 
 	switch (cmd) {
 		case FBIOGET_VBLANK:
@@ -561,11 +445,22 @@ static int matroxfb_dh_ioctl(struct inode* inode,
 					return -EFAULT;
 				return 0;
 			}
+		case FBIO_WAITFORVSYNC:
+			{
+				u_int32_t crt;
+
+				if (get_user(crt, (u_int32_t *)arg))
+					return -EFAULT;
+
+				if (crt != 0)
+					return -ENODEV;
+				return matroxfb_wait_for_sync(PMINFO 1);
+			}
 		case MATROXFB_SET_OUTPUT_MODE:
 		case MATROXFB_GET_OUTPUT_MODE:
 		case MATROXFB_GET_ALL_OUTPUTS:
 			{
-				return ACCESS_FBINFO(fbcon.fbops)->fb_ioctl(inode, file, cmd, arg, con, &ACCESS_FBINFO(fbcon));
+				return ACCESS_FBINFO(fbcon.fbops)->fb_ioctl(inode, file, cmd, arg, &ACCESS_FBINFO(fbcon));
 			}
 		case MATROXFB_SET_OUTPUT_CONNECTION:
 			{
@@ -610,7 +505,7 @@ static int matroxfb_dh_ioctl(struct inode* inode,
 				}
 				if (!changes)
 					return 0;
-				matroxfb_dh_switch(m2info->fbcon.currcon, info);
+				matroxfb_dh_set_par(info);
 				return 0;
 			}
 		case MATROXFB_GET_OUTPUT_CONNECTION:
@@ -675,45 +570,18 @@ static struct fb_ops matroxfb_dh_ops = {
 	.owner =	THIS_MODULE,
 	.fb_open =	matroxfb_dh_open,
 	.fb_release =	matroxfb_dh_release,
-	.fb_set_var =	matroxfb_dh_set_var,
-	.fb_get_cmap =	matroxfb_dh_get_cmap,
-	.fb_set_cmap =	matroxfb_dh_set_cmap,
+	.fb_check_var =	matroxfb_dh_check_var,
+	.fb_set_par =	matroxfb_dh_set_par,
 	.fb_setcolreg =	matroxfb_dh_setcolreg,
 	.fb_pan_display =matroxfb_dh_pan_display,
 	.fb_blank =	matroxfb_dh_blank,
 	.fb_ioctl =	matroxfb_dh_ioctl,
+	.fb_fillrect =	cfb_fillrect,
+	.fb_copyarea =	cfb_copyarea,
+	.fb_imageblit =	cfb_imageblit,
+	.fb_cursor =	soft_cursor,
 };
 
-static int matroxfb_dh_switch(int con, struct fb_info* info) {
-#define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
-	struct fb_cmap* cmap;
-	struct display* p;
-
-	if (m2info->fbcon.currcon >= 0) {
-		cmap = &m2info->currcon_display->cmap;
-		if (cmap->len) {
-			fb_get_cmap(cmap, 1, matroxfb_dh_getcolreg, info);
-		}
-	}
-	m2info->fbcon.currcon = con;
-	if (con < 0)
-		p = m2info->fbcon.disp;
-	else
-		p = fb_display + con;
-	m2info->currcon_display = p;
-	p->var.activate = FB_ACTIVATE_NOW;
-	matroxfb_dh_set_var(&p->var, con, info);
-	return 0;
-#undef m2info
-}
-
-static int matroxfb_dh_updatevar(int con, struct fb_info* info) {
-#define m2info (container_of(info, struct matroxfb_dh_fb_info, fbcon))
-	matroxfb_dh_pan_var(m2info, &fb_display[con].var);
-	return 0;
-#undef m2info
-}
-
 static struct fb_var_screeninfo matroxfb_dh_defined = {
 		640,480,640,480,/* W,H, virtual W,H */
 		0,0,		/* offset */
@@ -730,30 +598,17 @@ static struct fb_var_screeninfo matroxfb_dh_defined = {
 		39721L,48L,16L,33L,10L,
 		96L,2,0,	/* no sync info */
 		FB_VMODE_NONINTERLACED,
-		{0,0,0,0,0,0}
+		0, {0,0,0,0,0}
 };
 
 static int matroxfb_dh_regit(CPMINFO struct matroxfb_dh_fb_info* m2info) {
 #define minfo (m2info->primary_dev)
-	struct display* d;
 	void* oldcrtc2;
 
-	d = kmalloc(sizeof(*d), GFP_KERNEL);
-	if (!d) {
-		return -ENOMEM;
-	}
-
-	memset(d, 0, sizeof(*d));
-
-	strcpy(m2info->fbcon.modename, "MATROX CRTC2");
-	m2info->fbcon.changevar = NULL;
 	m2info->fbcon.fbops = &matroxfb_dh_ops;
-	m2info->fbcon.disp = d;
-	m2info->fbcon.switch_con = &matroxfb_dh_switch;
-	m2info->fbcon.updatevar = &matroxfb_dh_updatevar;
 	m2info->fbcon.flags = FBINFO_FLAG_DEFAULT;
 	m2info->fbcon.currcon = -1;
-	m2info->currcon_display = d;
+	m2info->fbcon.pseudo_palette = m2info->cmap;
 
 	if (mem < 64)
 		mem *= 1024;
@@ -763,7 +618,6 @@ static int matroxfb_dh_regit(CPMINFO struct matroxfb_dh_fb_info* m2info) {
 	if (ACCESS_FBINFO(video.len_usable) + mem <= ACCESS_FBINFO(video.len))
 		m2info->video.offbase = ACCESS_FBINFO(video.len) - mem;
 	else if (ACCESS_FBINFO(video.len) < mem) {
-		kfree(d);
 		return -ENOMEM;
 	} else { /* check yres on first head... */
 		m2info->video.borrowed = mem;
@@ -786,13 +640,12 @@ static int matroxfb_dh_regit(CPMINFO struct matroxfb_dh_fb_info* m2info) {
 		ACCESS_FBINFO(outputs[1]).src = MATROXFB_SRC_CRTC2;
 	}
 
-	matroxfb_dh_set_var(&matroxfb_dh_defined, -2, &m2info->fbcon);
+	matroxfb_dh_init_fix(m2info);
 	if (register_framebuffer(&m2info->fbcon)) {
-		kfree(d);
 		return -ENXIO;
 	}
 	if (m2info->fbcon.currcon < 0) {
-		matroxfb_dh_set_var(&matroxfb_dh_defined, -1, &m2info->fbcon);
+		fb_set_var(&m2info->fbcon, &matroxfb_dh_defined);
 	}
 	down_write(&ACCESS_FBINFO(crtc2.lock));
 	oldcrtc2 = ACCESS_FBINFO(crtc2.info);
@@ -840,7 +693,6 @@ static void matroxfb_dh_deregisterfb(struct matroxfb_dh_fb_info* m2info) {
 		}
 		id = m2info->fbcon.node;
 		unregister_framebuffer(&m2info->fbcon);
-		kfree(m2info->fbcon.disp);
 		/* return memory back to primary head */
 		ACCESS_FBINFO(video.len_usable) += m2info->video.borrowed;
 		printk(KERN_INFO "matroxfb_crtc2: fb%u unregistered\n", id);

drivers/video/matrox/matroxfb_crtc2.h

@@ -27,19 +27,10 @@ struct matroxfb_dh_fb_info {
 		unsigned int	len;
 			      } mmio;
 
-	struct display*		currcon_display;
-	
 	int			interlaced:1;
 
-	union {
-#ifdef FBCON_HAS_CFB16
-		u_int16_t	cfb16[16];
-#endif
-#ifdef FBCON_HAS_CFB32
-		u_int32_t	cfb32[16];
-#endif
-	} cmap;
-	struct { unsigned red, green, blue, transp; } palette[16];
+	u_int32_t cmap[17];
+	struct { unsigned red, green, blue, transp; } palette[17];
 };
 
 #endif /* __MATROXFB_CRTC2_H__ */

drivers/video/matrox/matroxfb_g450.c

@@ -6,7 +6,7 @@
  *
  * Portions Copyright (c) 2001 Matrox Graphics Inc.
  *
- * Version: 1.64 2002/06/02
+ * Version: 1.65 2002/08/14
  *
  * See matroxfb_base.c for contributors.
  *
@@ -20,6 +20,88 @@
 #include <asm/uaccess.h>
 #include <asm/div64.h>
 
+/* Definition of the various controls */
+struct mctl {
+	struct v4l2_queryctrl desc;
+	size_t control;
+};
+
+#define BLMIN	0xF3
+#define WLMAX	0x3FF
+
+static const struct mctl g450_controls[] =
+{	{ { V4L2_CID_BRIGHTNESS, V4L2_CTRL_TYPE_INTEGER, 
+	  "brightness",
+	  0, WLMAX-BLMIN, 1, 370-BLMIN, 
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.brightness) },
+	{ { V4L2_CID_CONTRAST, V4L2_CTRL_TYPE_INTEGER, 
+	  "contrast",
+	  0, 1023, 1, 127, 
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.contrast) },
+	{ { V4L2_CID_SATURATION, V4L2_CTRL_TYPE_INTEGER,
+	  "saturation",
+	  0, 255, 1, 165, 
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.saturation) },
+	{ { V4L2_CID_HUE, V4L2_CTRL_TYPE_INTEGER,
+	  "hue",
+	  0, 255, 1, 0, 
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.hue) },
+	{ { MATROXFB_CID_TESTOUT, V4L2_CTRL_TYPE_BOOLEAN,
+	  "test output",
+	  0, 1, 1, 0, 
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.testout) },
+};
+
+#define G450CTRLS (sizeof(g450_controls)/sizeof(g450_controls[0]))
+
+/* Return: positive number: id found
+           -EINVAL:         id not found, return failure
+	   -ENOENT:         id not found, create fake disabled control */
+static int get_ctrl_id(__u32 v4l2_id) {
+	int i;
+
+	for (i = 0; i < G450CTRLS; i++) {
+		if (v4l2_id < g450_controls[i].desc.id) {
+			if (g450_controls[i].desc.id == 0x08000000) {
+				return -EINVAL;
+			}
+			return -ENOENT;
+		}
+		if (v4l2_id == g450_controls[i].desc.id) {
+			return i;
+		}
+	}
+	return -EINVAL;
+}
+
+static inline int* get_ctrl_ptr(WPMINFO unsigned int idx) {
+	return (int*)((char*)MINFO + g450_controls[idx].control);
+}
+
+static void tvo_fill_defaults(WPMINFO2) {
+	unsigned int i;
+	
+	for (i = 0; i < G450CTRLS; i++) {
+		*get_ctrl_ptr(PMINFO i) = g450_controls[i].desc.default_value;
+	}
+}
+
+static int cve2_get_reg(WPMINFO int reg) {
+	unsigned long flags;
+	int val;
+	
+	matroxfb_DAC_lock_irqsave(flags);
+	matroxfb_DAC_out(PMINFO 0x87, reg);
+	val = matroxfb_DAC_in(PMINFO 0x88);
+	matroxfb_DAC_unlock_irqrestore(flags);
+	return val;
+}
+
 static void cve2_set_reg(WPMINFO int reg, int val) {
 	unsigned long flags;
 
@@ -29,6 +111,110 @@ static void cve2_set_reg(WPMINFO int reg, int val) {
 	matroxfb_DAC_unlock_irqrestore(flags);
 }
 
+static void cve2_set_reg10(WPMINFO int reg, int val) {
+	unsigned long flags;
+
+	matroxfb_DAC_lock_irqsave(flags);
+	matroxfb_DAC_out(PMINFO 0x87, reg);
+	matroxfb_DAC_out(PMINFO 0x88, val >> 2);
+	matroxfb_DAC_out(PMINFO 0x87, reg + 1);
+	matroxfb_DAC_out(PMINFO 0x88, val & 3);
+	matroxfb_DAC_unlock_irqrestore(flags);
+}
+
+static void g450_compute_bwlevel(CPMINFO int *bl, int *wl) {
+	const int b = ACCESS_FBINFO(altout.tvo_params.brightness) + BLMIN;
+	const int c = ACCESS_FBINFO(altout.tvo_params.contrast);
+
+	*bl = max(b - c, BLMIN);
+	*wl = min(b + c, WLMAX);
+}
+
+static int g450_query_ctrl(void* md, struct v4l2_queryctrl *p) {
+	int i;
+	
+	i = get_ctrl_id(p->id);
+	if (i >= 0) {
+		*p = g450_controls[i].desc;
+		return 0;
+	}
+	if (i == -ENOENT) {
+		static const struct v4l2_queryctrl disctrl = 
+			{ .flags = V4L2_CTRL_FLAG_DISABLED };
+			
+		i = p->id;
+		*p = disctrl;
+		p->id = i;
+		sprintf(p->name, "Ctrl #%08X", i);
+		return 0;
+	}
+	return -EINVAL;
+}
+
+static int g450_set_ctrl(void* md, struct v4l2_control *p) {
+	int i;
+	MINFO_FROM(md);
+	
+	i = get_ctrl_id(p->id);
+	if (i < 0) return -EINVAL;
+
+	/*
+	 * Check if changed.
+	 */
+	if (p->value == *get_ctrl_ptr(PMINFO i)) return 0;
+
+	/*
+	 * Check limits.
+	 */
+	if (p->value > g450_controls[i].desc.maximum) return -EINVAL;
+	if (p->value < g450_controls[i].desc.minimum) return -EINVAL;
+
+	/*
+	 * Store new value.
+	 */
+	*get_ctrl_ptr(PMINFO i) = p->value;
+
+	switch (p->id) {
+		case V4L2_CID_BRIGHTNESS:
+		case V4L2_CID_CONTRAST:
+			{
+				int blacklevel, whitelevel;
+				g450_compute_bwlevel(PMINFO &blacklevel, &whitelevel);
+				cve2_set_reg10(PMINFO 0x0e, blacklevel);
+				cve2_set_reg10(PMINFO 0x1e, whitelevel);
+			}
+			break;
+		case V4L2_CID_SATURATION:
+			cve2_set_reg(PMINFO 0x20, p->value);
+			cve2_set_reg(PMINFO 0x22, p->value);
+			break;
+		case V4L2_CID_HUE:
+			cve2_set_reg(PMINFO 0x25, p->value);
+			break;
+		case MATROXFB_CID_TESTOUT:
+			{
+				unsigned char val = cve2_get_reg (PMINFO 0x05);
+				if (p->value) val |=  0x02;
+				else          val &= ~0x02;
+				cve2_set_reg(PMINFO 0x05, val);
+			}
+			break;
+	}
+	
+
+	return 0;
+}
+
+static int g450_get_ctrl(void* md, struct v4l2_control *p) {
+	int i;
+	MINFO_FROM(md);
+	
+	i = get_ctrl_id(p->id);
+	if (i < 0) return -EINVAL;
+	p->value = *get_ctrl_ptr(PMINFO i);
+	return 0;
+}
+
 struct output_desc {
 	unsigned int	h_vis;
 	unsigned int	h_f_porch;
@@ -329,6 +515,23 @@ static int matroxfb_g450_compute(void* md, struct my_timming* mt) {
 		const struct output_desc* outd;
 
 		cve2_init_TVdata(ACCESS_FBINFO(outputs[1]).mode, &ACCESS_FBINFO(hw).maven, &outd);
+		{
+			int blacklevel, whitelevel;
+			g450_compute_bwlevel(PMINFO &blacklevel, &whitelevel);
+			ACCESS_FBINFO(hw).maven.regs[0x0E] = blacklevel >> 2;
+			ACCESS_FBINFO(hw).maven.regs[0x0F] = blacklevel & 3;
+			ACCESS_FBINFO(hw).maven.regs[0x1E] = whitelevel >> 2;
+			ACCESS_FBINFO(hw).maven.regs[0x1F] = whitelevel & 3;
+
+			ACCESS_FBINFO(hw).maven.regs[0x20] =
+			ACCESS_FBINFO(hw).maven.regs[0x22] = ACCESS_FBINFO(altout.tvo_params.saturation);
+
+			ACCESS_FBINFO(hw).maven.regs[0x25] = ACCESS_FBINFO(altout.tvo_params.hue);
+
+			if (ACCESS_FBINFO(altout.tvo_params.testout)) {
+				ACCESS_FBINFO(hw).maven.regs[0x05] |= 0x02;
+			}
+		}
 		computeRegs(PMINFO &ACCESS_FBINFO(hw).maven, mt, outd);
 	} else if (mt->mnp < 0) {
 		/* We must program clocks before CRTC2, otherwise interlaced mode
@@ -374,6 +577,9 @@ static struct matrox_altout matroxfb_g450_altout = {
 	.compute	= matroxfb_g450_compute,
 	.program	= matroxfb_g450_program,
 	.verifymode	= matroxfb_g450_verify_mode,
+	.getqueryctrl	= g450_query_ctrl,
+	.getctrl	= g450_get_ctrl,
+	.setctrl	= g450_set_ctrl,
 };
 
 static struct matrox_altout matroxfb_g450_dvi = {
@@ -384,6 +590,7 @@ static struct matrox_altout matroxfb_g450_dvi = {
 void matroxfb_g450_connect(WPMINFO2) {
 	if (ACCESS_FBINFO(devflags.g450dac)) {
 		down_write(&ACCESS_FBINFO(altout.lock));
+		tvo_fill_defaults(PMINFO2);
 		ACCESS_FBINFO(outputs[1]).src = MATROXFB_SRC_CRTC1;
 		ACCESS_FBINFO(outputs[1]).data = MINFO;
 		ACCESS_FBINFO(outputs[1]).output = &matroxfb_g450_altout;

drivers/video/matrox/matroxfb_maven.c

@@ -6,7 +6,7 @@
  *
  * Portions Copyright (c) 2001 Matrox Graphics Inc.
  *
- * Version: 1.64 2002/06/10
+ * Version: 1.65 2002/08/14
  *
  * See matroxfb_base.c for contributors.
  *
@@ -25,12 +25,118 @@
 #define MGATVO_B	1
 #define MGATVO_C	2
 
+static const struct maven_gamma {
+  unsigned char reg83;
+  unsigned char reg84;
+  unsigned char reg85;
+  unsigned char reg86;
+  unsigned char reg87;
+  unsigned char reg88;
+  unsigned char reg89;
+  unsigned char reg8a;
+  unsigned char reg8b;
+} maven_gamma[] = {
+  { 131, 57, 223, 15, 117, 212, 251, 91, 156},
+  { 133, 61, 128, 63, 180, 147, 195, 100, 180},
+  { 131, 19, 63, 31, 50, 66, 171, 64, 176},
+  { 0, 0, 0, 31, 16, 16, 16, 100, 200},
+  { 8, 23, 47, 73, 147, 244, 220, 80, 195},
+  { 22, 43, 64, 80, 147, 115, 58, 85, 168},
+  { 34, 60, 80, 214, 147, 212, 188, 85, 167},
+  { 45, 77, 96, 216, 147, 99, 91, 85, 159},
+  { 56, 76, 112, 107, 147, 212, 148, 64, 144},
+  { 65, 91, 128, 137, 147, 196, 17, 69, 148},
+  { 72, 104, 136, 138, 147, 180, 245, 73, 147},
+  { 87, 116, 143, 126, 16, 83, 229, 77, 144},
+  { 95, 119, 152, 254, 244, 83, 221, 77, 151},
+  { 100, 129, 159, 156, 244, 148, 197, 77, 160},
+  { 105, 141, 167, 247, 244, 132, 181, 84, 166},
+  { 105, 147, 168, 247, 244, 245, 181, 90, 170},
+  { 120, 153, 175, 248, 212, 229, 165, 90, 180},
+  { 119, 156, 176, 248, 244, 229, 84, 74, 160},
+  { 119, 158, 183, 248, 244, 229, 149, 78, 165}
+};
+
+/* Definition of the various controls */
+struct mctl {
+	struct v4l2_queryctrl desc;
+	size_t control;
+};
+
+#define BLMIN	0x0FF
+#define WLMAX	0x3FF
+
+static const struct mctl maven_controls[] =
+{	{ { V4L2_CID_BRIGHTNESS, V4L2_CTRL_TYPE_INTEGER,
+	  "brightness",
+	  0, WLMAX - BLMIN, 1, 379 - BLMIN, 
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.brightness) },
+	{ { V4L2_CID_CONTRAST, V4L2_CTRL_TYPE_INTEGER,
+	  "contrast",
+	  0, 1023, 1, 127,
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.contrast) },
+	{ { V4L2_CID_SATURATION, V4L2_CTRL_TYPE_INTEGER,
+	  "saturation",
+	  0, 255, 1, 155,
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.saturation) },
+	{ { V4L2_CID_HUE, V4L2_CTRL_TYPE_INTEGER,
+	  "hue",
+	  0, 255, 1, 0,
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.hue) },
+	{ { V4L2_CID_GAMMA, V4L2_CTRL_TYPE_INTEGER,
+	  "gamma",
+	  0, sizeof(maven_gamma)/sizeof(maven_gamma[0])-1, 1, 3,
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.gamma) },
+	{ { MATROXFB_CID_TESTOUT, V4L2_CTRL_TYPE_BOOLEAN,
+	  "test output",
+	  0, 1, 1, 0, 
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.testout) },
+	{ { MATROXFB_CID_DEFLICKER, V4L2_CTRL_TYPE_INTEGER,
+	  "deflicker mode",
+	  0, 2, 1, 0,
+	  0,
+	}, offsetof(struct matrox_fb_info, altout.tvo_params.deflicker) },
+
+};
+
+#define MAVCTRLS (sizeof(maven_controls)/sizeof(maven_controls[0]))
+
+/* Return: positive number: id found
+           -EINVAL:         id not found, return failure
+	   -ENOENT:         id not found, create fake disabled control */
+static int get_ctrl_id(__u32 v4l2_id) {
+	int i;
+
+	for (i = 0; i < MAVCTRLS; i++) {
+		if (v4l2_id < maven_controls[i].desc.id) {
+			if (maven_controls[i].desc.id == 0x08000000) {
+				return -EINVAL;
+			}
+			return -ENOENT;
+		}
+		if (v4l2_id == maven_controls[i].desc.id) {
+			return i;
+		}
+	}
+	return -EINVAL;
+}
+
 struct maven_data {
 	struct matrox_fb_info*		primary_head;
 	struct i2c_client*		client;
 	int				version;
 };
 
+static int* get_ctrl_ptr(struct maven_data* md, int idx) {
+	return (int*)((char*)(md->primary_head) + maven_controls[idx].control);
+}
+
 static int maven_get_reg(struct i2c_client* c, char reg) {
 	char dst;
 	struct i2c_msg msgs[] = {{ c->addr, I2C_M_REV_DIR_ADDR, sizeof(reg), &reg },
@@ -115,7 +221,7 @@ static int matroxfb_PLL_mavenclock(const struct matrox_pll_features2* pll,
 	unsigned int scrlen;
 	unsigned int fmax;
 
-	DBG("PLL_calcclock")
+	DBG(__FUNCTION__)
 
 	scrlen = htotal * (vtotal - 1);
 	fwant = htotal * vtotal;
@@ -219,9 +325,34 @@ static unsigned char maven_compute_deflicker (const struct maven_data* md) {
 	unsigned char df;
 	
 	df = (md->version == MGATVO_B?0x40:0x00);
+	switch (md->primary_head->altout.tvo_params.deflicker) {
+		case 0:
+/*			df |= 0x00; */
+			break;
+		case 1:
+			df |= 0xB1;
+			break;
+		case 2:
+			df |= 0xA2;
+			break;
+	}
 	return df;
 }
 
+static void maven_compute_bwlevel (const struct maven_data* md,
+				   int *bl, int *wl) {
+	const int b = md->primary_head->altout.tvo_params.brightness + BLMIN;
+	const int c = md->primary_head->altout.tvo_params.contrast;
+
+	*bl = max(b - c, BLMIN);
+	*wl = min(b + c, WLMAX);
+}
+
+static const struct maven_gamma* maven_compute_gamma (const struct maven_data* md) {
+ 	return maven_gamma + md->primary_head->altout.tvo_params.gamma;
+}
+
+
 static void maven_init_TVdata(const struct maven_data* md, struct mavenregs* data) {
 	static struct mavenregs palregs = { {
 		0x2A, 0x09, 0x8A, 0xCB,	/* 00: chroma subcarrier */
@@ -336,18 +467,40 @@ static void maven_init_TVdata(const struct maven_data* md, struct mavenregs* dat
 
 	/* Set deflicker */
 	data->regs[0x93] = maven_compute_deflicker(md);
+ 
+	/* set gamma */
+	{
+		const struct maven_gamma* g;
+		g = maven_compute_gamma(md);
+		data->regs[0x83] = g->reg83;
+		data->regs[0x84] = g->reg84;
+		data->regs[0x85] = g->reg85;
+		data->regs[0x86] = g->reg86;
+		data->regs[0x87] = g->reg87;
+		data->regs[0x88] = g->reg88;
+		data->regs[0x89] = g->reg89;
+		data->regs[0x8A] = g->reg8a;
+		data->regs[0x8B] = g->reg8b;
+	}
+ 
+	/* Set contrast / brightness */
+	{
+		int bl, wl;
+		maven_compute_bwlevel (md, &bl, &wl);
+		data->regs[0x0e] = bl >> 2;
+		data->regs[0x0f] = bl & 3;
+		data->regs[0x1e] = wl >> 2;
+		data->regs[0x1f] = wl & 3;
+	}
 
-	/* gamma correction registers */
-	data->regs[0x83] = 0x00;
-	data->regs[0x84] = 0x00;
-	data->regs[0x85] = 0x00;
-	data->regs[0x86] = 0x1F;
-	data->regs[0x87] = 0x10;
-	data->regs[0x88] = 0x10;
-	data->regs[0x89] = 0x10;
-	data->regs[0x8A] = 0x64;	/* 100 */
-	data->regs[0x8B] = 0xC8;	/* 200 */
-
+	/* Set saturation */
+	{
+		data->regs[0x20] =
+		data->regs[0x22] = ACCESS_FBINFO(altout.tvo_params.saturation);
+	}
+ 
+	/* Set HUE */
+	data->regs[0x25] = ACCESS_FBINFO(altout.tvo_params.hue);
 	return;
 }
 
@@ -859,16 +1012,119 @@ static inline int maven_resync(struct maven_data* md) {
 	return 0;
 }
 
-static int maven_verify_output_mode(struct maven_data* md, u_int32_t arg) {
-	switch (arg) {
-		case MATROXFB_OUTPUT_MODE_PAL:
-		case MATROXFB_OUTPUT_MODE_NTSC:
-		case MATROXFB_OUTPUT_MODE_MONITOR:
-			return 0;
+static int maven_get_queryctrl (struct maven_data* md, 
+				struct v4l2_queryctrl *p) {
+	int i;
+	
+	i = get_ctrl_id(p->id);
+	if (i >= 0) {
+		*p = maven_controls[i].desc;
+		return 0;
+	}
+	if (i == -ENOENT) {
+		static const struct v4l2_queryctrl disctrl = 
+			{ .flags = V4L2_CTRL_FLAG_DISABLED };
+			
+		i = p->id;
+		*p = disctrl;
+		p->id = i;
+		sprintf(p->name, "Ctrl #%08X", i);
+		return 0;
 	}
 	return -EINVAL;
 }
 
+static int maven_set_control (struct maven_data* md, 
+			      struct v4l2_control *p) {
+	int i;
+	
+	i = get_ctrl_id(p->id);
+	if (i < 0) return -EINVAL;
+
+	/*
+	 * Check if changed.
+	 */
+	if (p->value == *get_ctrl_ptr(md, i)) return 0;
+
+	/*
+	 * Check limits.
+	 */
+	if (p->value > maven_controls[i].desc.maximum) return -EINVAL;
+	if (p->value < maven_controls[i].desc.minimum) return -EINVAL;
+
+	/*
+	 * Store new value.
+	 */
+	*get_ctrl_ptr(md, i) = p->value;
+
+	switch (p->id) {
+		case V4L2_CID_BRIGHTNESS:
+		case V4L2_CID_CONTRAST:
+		{
+		  int blacklevel, whitelevel;
+		  maven_compute_bwlevel(md, &blacklevel, &whitelevel);
+		  blacklevel = (blacklevel >> 2) | ((blacklevel & 3) << 8);
+		  whitelevel = (whitelevel >> 2) | ((whitelevel & 3) << 8);
+		  maven_set_reg_pair(md->client, 0x0e, blacklevel);
+		  maven_set_reg_pair(md->client, 0x1e, whitelevel);
+		}
+		break;
+		case V4L2_CID_SATURATION:
+		{
+		  maven_set_reg(md->client, 0x20, p->value);
+		  maven_set_reg(md->client, 0x22, p->value);
+		}
+		break;
+		case V4L2_CID_HUE:
+		{
+		  maven_set_reg(md->client, 0x25, p->value);
+		}
+		break;
+		case V4L2_CID_GAMMA:
+		{
+		  const struct maven_gamma* g;
+		  g = maven_compute_gamma(md);
+		  maven_set_reg(md->client, 0x83, g->reg83);
+		  maven_set_reg(md->client, 0x84, g->reg84);
+		  maven_set_reg(md->client, 0x85, g->reg85);
+		  maven_set_reg(md->client, 0x86, g->reg86);
+		  maven_set_reg(md->client, 0x87, g->reg87);
+		  maven_set_reg(md->client, 0x88, g->reg88);
+		  maven_set_reg(md->client, 0x89, g->reg89);
+		  maven_set_reg(md->client, 0x8a, g->reg8a);
+		  maven_set_reg(md->client, 0x8b, g->reg8b);
+		}
+		break;
+		case MATROXFB_CID_TESTOUT:
+		{
+			unsigned char val 
+			  = maven_get_reg (md->client,0x8d);
+			if (p->value) val |= 0x10;
+			else          val &= ~0x10;
+			maven_set_reg (md->client, 0x8d, val);
+		}
+		break;
+		case MATROXFB_CID_DEFLICKER:
+		{
+		  maven_set_reg(md->client, 0x93, maven_compute_deflicker(md));
+		}
+		break;
+	}
+	
+
+	return 0;
+}
+
+static int maven_get_control (struct maven_data* md, 
+			      struct v4l2_control *p) {
+	int i;
+	
+	i = get_ctrl_id(p->id);
+	if (i < 0) return -EINVAL;
+	p->value = *get_ctrl_ptr(md, i);
+	return 0;
+}
+
 /******************************************************/
 
 static int maven_out_compute(void* md, struct my_timming* mt) {
@@ -892,7 +1148,25 @@ static int maven_out_start(void* md) {
 }
 
 static int maven_out_verify_mode(void* md, u_int32_t arg) {
-	return maven_verify_output_mode(md, arg);
+	switch (arg) {
+		case MATROXFB_OUTPUT_MODE_PAL:
+		case MATROXFB_OUTPUT_MODE_NTSC:
+		case MATROXFB_OUTPUT_MODE_MONITOR:
+			return 0;
+	}
+	return -EINVAL;
+}
+
+static int maven_out_get_queryctrl(void* md, struct v4l2_queryctrl* p) {
+        return maven_get_queryctrl(md, p);
+}
+
+static int maven_out_get_ctrl(void* md, struct v4l2_control* p) {
+	return maven_get_control(md, p);
+}
+
+static int maven_out_set_ctrl(void* md, struct v4l2_control* p) {
+	return maven_set_control(md, p);
 }
 
 static struct matrox_altout maven_altout = {
@@ -901,12 +1175,14 @@ static struct matrox_altout maven_altout = {
 	.program	= maven_out_program,
 	.start		= maven_out_start,
 	.verifymode	= maven_out_verify_mode,
+	.getqueryctrl	= maven_out_get_queryctrl,
+	.getctrl	= maven_out_get_ctrl,
+	.setctrl	= maven_out_set_ctrl,
 };
 
 static int maven_init_client(struct i2c_client* clnt) {
-	struct i2c_adapter* a = clnt->adapter;
-	struct maven_data* md = clnt->data;
-	MINFO_FROM(container_of(a, struct i2c_bit_adapter, adapter)->minfo);
+	struct maven_data* md = i2c_get_clientdata(clnt);
+	MINFO_FROM(container_of(clnt->adapter, struct i2c_bit_adapter, adapter)->minfo);
 
 	md->primary_head = MINFO;
 	md->client = clnt;
@@ -918,14 +1194,26 @@ static int maven_init_client(struct i2c_client* clnt) {
 	up_write(&ACCESS_FBINFO(altout.lock));
 	if (maven_get_reg(clnt, 0xB2) < 0x14) {
 		md->version = MGATVO_B;
+		/* Tweak some things for this old chip */
 	} else {
 		md->version = MGATVO_C;
 	}
+	/*
+	 * Set all parameters to its initial values.
+	 */
+	{
+		unsigned int i;
+
+		for (i = 0; i < MAVCTRLS; ++i) {
+			*get_ctrl_ptr(md, i) = maven_controls[i].desc.default_value;
+		}
+	}
+
 	return 0;
 }
 
 static int maven_shutdown_client(struct i2c_client* clnt) {
-	struct maven_data* md = clnt->data;
+	struct maven_data* md = i2c_get_clientdata(clnt);
 
 	if (md->primary_head) {
 		MINFO_FROM(md->primary_head);
@@ -947,8 +1235,7 @@ I2C_CLIENT_INSMOD;
 
 static struct i2c_driver maven_driver;
 
-static int maven_detect_client(struct i2c_adapter* adapter, int address, unsigned short flags,
-		int kind) {
+static int maven_detect_client(struct i2c_adapter* adapter, int address, int kind) {
 	int err = 0;
 	struct i2c_client* new_client;
 	struct maven_data* data;
@@ -963,15 +1250,12 @@ static int maven_detect_client(struct i2c_adapter* adapter, int address, unsigne
 		goto ERROR0;
 	}
 	data = (struct maven_data*)(new_client + 1);
-	new_client->data = data;
+	i2c_set_clientdata(new_client, data);
 	new_client->addr = address;
 	new_client->adapter = adapter;
 	new_client->driver = &maven_driver;
 	new_client->flags = 0;
-	if (kind < 0) {
-		;
-	}
-	strcpy(new_client->name, "maven client");
+	strcpy(new_client->dev.name, "maven client");
 	if ((err = i2c_attach_client(new_client)))
 		goto ERROR3;
 	err = maven_init_client(new_client);
@@ -1015,6 +1299,7 @@ static struct i2c_driver maven_driver={
 	.flags		= I2C_DF_NOTIFY,
 	.attach_adapter	= maven_attach_adapter,
 	.detach_client	= maven_detach_client,
+	.command	= maven_command,
 };
 
 /* ************************** */

drivers/video/matrox/matroxfb_misc.c

@@ -88,52 +88,14 @@
 #include <linux/interrupt.h>
 #include <linux/matroxfb.h>
 
-void matroxfb_createcursorshape(WPMINFO struct display* p, int vmode) {
-	unsigned int h;
-	unsigned int cu, cd;
-
-	h = fontheight(p);
-
-	if (vmode & FB_VMODE_DOUBLE)
-		h *= 2;
-	cd = h;
-	if (cd >= 10)
-		cd--;
-	switch (ACCESS_FBINFO(cursor.type) = (p->conp->vc_cursor_type & CUR_HWMASK)) {
-		case CUR_NONE:
-				cu = cd;
-				break;
-		case CUR_UNDERLINE:
-				cu = cd - 2;
-				break;
-		case CUR_LOWER_THIRD:
-				cu = (h * 2) / 3;
-				break;
-		case CUR_LOWER_HALF:
-				cu = h / 2;
-				break;
-		case CUR_TWO_THIRDS:
-				cu = h / 3;
-				break;
-		case CUR_BLOCK:
-		default:
-				cu = 0;
-				cd = h;
-				break;
-	}
-	ACCESS_FBINFO(cursor.w) = fontwidth(p);
-	ACCESS_FBINFO(cursor.u) = cu;
-	ACCESS_FBINFO(cursor.d) = cd;
-}
-
 void matroxfb_DAC_out(CPMINFO int reg, int val) {
-	DBG_REG("outDAC");
+	DBG_REG(__FUNCTION__)
 	mga_outb(M_RAMDAC_BASE+M_X_INDEX, reg);
 	mga_outb(M_RAMDAC_BASE+M_X_DATAREG, val);
 }
 
 int matroxfb_DAC_in(CPMINFO int reg) {
-	DBG_REG("inDAC");
+	DBG_REG(__FUNCTION__)
 	mga_outb(M_RAMDAC_BASE+M_X_INDEX, reg);
 	return mga_inb(M_RAMDAC_BASE+M_X_DATAREG);
 }
@@ -141,7 +103,7 @@ int matroxfb_DAC_in(CPMINFO int reg) {
 void matroxfb_var2my(struct fb_var_screeninfo* var, struct my_timming* mt) {
 	unsigned int pixclock = var->pixclock;
 
-	DBG("var2my")
+	DBG(__FUNCTION__)
 
 	if (!pixclock) pixclock = 10000;	/* 10ns = 100MHz */
 	mt->pixclock = 1000000000 / pixclock;
@@ -168,7 +130,7 @@ int matroxfb_PLL_calcclock(const struct matrox_pll_features* pll, unsigned int f
 	unsigned int fwant;
 	unsigned int p;
 
-	DBG("PLL_calcclock")
+	DBG(__FUNCTION__)
 
 	fwant = freq;
 
@@ -221,23 +183,18 @@ int matroxfb_PLL_calcclock(const struct matrox_pll_features* pll, unsigned int f
 	return bestvco;
 }
 
-int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
+int matroxfb_vgaHWinit(WPMINFO struct my_timming* m) {
 	unsigned int hd, hs, he, hbe, ht;
-	unsigned int vd, vs, ve, vt;
+	unsigned int vd, vs, ve, vt, lc;
 	unsigned int wd;
 	unsigned int divider;
 	int i;
-	int text = ACCESS_FBINFO(fbcon).fix.type == FB_TYPE_TEXT;
 	int fwidth;
 	struct matrox_hw_state * const hw = &ACCESS_FBINFO(hw);
 
-	if (text) {
-		fwidth = fontwidth(p);
-		if (!fwidth) fwidth = 8;
-	} else
-		fwidth = 8;
+	fwidth = 8;
 
-	DBG("vgaHWinit")
+	DBG(__FUNCTION__)
 
 	hw->SEQ[0] = 0x00;
 	if (fwidth == 9)
@@ -246,10 +203,7 @@ int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
 		hw->SEQ[1] = 0x01;	/* or 0x09 */
 	hw->SEQ[2] = 0x0F;	/* bitplanes */
 	hw->SEQ[3] = 0x00;
-	if (text)
-		hw->SEQ[4] = 0x02;
-	else
-		hw->SEQ[4] = 0x0E;
+	hw->SEQ[4] = 0x0E;
 	/* CRTC 0..7, 9, 16..19, 21, 22 are reprogrammed by Matrox Millennium code... Hope that by MGA1064 too */
 	if (m->dblscan) {
 		m->VTotal <<= 1;
@@ -270,24 +224,15 @@ int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
 	hw->GCTL[2] = 0x00;
 	hw->GCTL[3] = 0x00;
 	hw->GCTL[4] = 0x00;
-	if (text) {
-		hw->GCTL[5] = 0x10;
-		hw->GCTL[6] = 0x02;
-	} else {
-		hw->GCTL[5] = 0x40;
-		hw->GCTL[6] = 0x05;
-	}
+	hw->GCTL[5] = 0x40;
+	hw->GCTL[6] = 0x05;
 	hw->GCTL[7] = 0x0F;
 	hw->GCTL[8] = 0xFF;
 
 	/* Whole ATTR is ignored in PowerGraphics mode */
 	for (i = 0; i < 16; i++)
 		hw->ATTR[i] = i;
-	if (text) {
-		hw->ATTR[16] = 0x04;
-	} else {
-		hw->ATTR[16] = 0x41;
-	}
+	hw->ATTR[16] = 0x41;
 	hw->ATTR[17] = 0xFF;
 	hw->ATTR[18] = 0x0F;
 	if (fwidth == 9)
@@ -296,22 +241,14 @@ int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
 		hw->ATTR[19] = 0x00;
 	hw->ATTR[20] = 0x00;
 
-	if (text) {
-		hd = m->HDisplay / fwidth;
-		hs = m->HSyncStart / fwidth;
-		he = m->HSyncEnd / fwidth;
-		ht = m->HTotal / fwidth;
-		divider = 8;
-	} else {
-		hd = m->HDisplay >> 3;
-		hs = m->HSyncStart >> 3;
-		he = m->HSyncEnd >> 3;
-		ht = m->HTotal >> 3;
-		/* standard timmings are in 8pixels, but for interleaved we cannot */
-		/* do it for 4bpp (because of (4bpp >> 1(interleaved))/4 == 0) */
-		/* using 16 or more pixels per unit can save us */
-		divider = ACCESS_FBINFO(curr.final_bppShift);
-	}
+	hd = m->HDisplay >> 3;
+	hs = m->HSyncStart >> 3;
+	he = m->HSyncEnd >> 3;
+	ht = m->HTotal >> 3;
+	/* standard timmings are in 8pixels, but for interleaved we cannot */
+	/* do it for 4bpp (because of (4bpp >> 1(interleaved))/4 == 0) */
+	/* using 16 or more pixels per unit can save us */
+	divider = ACCESS_FBINFO(curr.final_bppShift);
 	while (divider & 3) {
 		hd >>= 1;
 		hs >>= 1;
@@ -336,16 +273,12 @@ int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
 	vs = m->VSyncStart - 1;
 	ve = m->VSyncEnd - 1;
 	vt = m->VTotal - 2;
+	lc = vd;
 	/* G200 cannot work with (ht & 7) == 6 */
 	if (((ht & 0x07) == 0x06) || ((ht & 0x0F) == 0x04))
 		ht++;
-	if (text) {
-		hbe = ht - 1;
-		wd = p->var.xres_virtual / (fwidth * 2);
-	} else {
-		hbe = ht;
-		wd = p->var.xres_virtual * ACCESS_FBINFO(curr.final_bppShift) / 64;
-	}
+	hbe = ht;
+	wd = ACCESS_FBINFO(fbcon).var.xres_virtual * ACCESS_FBINFO(curr.final_bppShift) / 64;
 
 	hw->CRTCEXT[0] = 0;
 	hw->CRTCEXT[5] = 0;
@@ -367,11 +300,9 @@ int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
 	hw->CRTCEXT[2] =  ((vt & 0xC00) >> 10) |
 			  ((vd & 0x400) >>  8) |	/* disp end */
 			  ((vd & 0xC00) >>  7) |	/* vblanking start */
-			  ((vs & 0xC00) >>  5);
-	if (text)
-		hw->CRTCEXT[3] = 0x00;
-	else
-		hw->CRTCEXT[3] = (divider - 1) | 0x80;
+			  ((vs & 0xC00) >>  5) |
+			  ((lc & 0x400) >>  3);
+	hw->CRTCEXT[3] = (divider - 1) | 0x80;
 	hw->CRTCEXT[4] = 0;
 
 	hw->CRTC[0] = ht-4;
@@ -380,21 +311,18 @@ int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
 	hw->CRTC[3] = (hbe & 0x1F) | 0x80;
 	hw->CRTC[4] = hs;
 	hw->CRTC[5] = ((hbe & 0x20) << 2) | (he & 0x1F);
-	if (text)
-		hw->CRTC[5] |= 0x60;	/* delay sync for 3 clocks (to same picture position on MGA and VGA) */
 	hw->CRTC[6] = vt & 0xFF;
 	hw->CRTC[7] = ((vt & 0x100) >> 8) |
 		      ((vd & 0x100) >> 7) |
 		      ((vs & 0x100) >> 6) |
 		      ((vd & 0x100) >> 5) |
-		      0x10                |
+		      ((lc & 0x100) >> 4) |
 		      ((vt & 0x200) >> 4) |
 		      ((vd & 0x200) >> 3) |
 		      ((vs & 0x200) >> 2);
 	hw->CRTC[8] = 0x00;
-	hw->CRTC[9] = ((vd & 0x200) >> 4) | 0x40;
-	if (text)
-		hw->CRTC[9] |= fontheight(p) - 1;
+	hw->CRTC[9] = ((vd & 0x200) >> 4) |
+		      ((lc & 0x200) >> 3);
 	if (m->dblscan && !m->interlaced)
 		hw->CRTC[9] |= 0x80;
 	for (i = 10; i < 16; i++)
@@ -406,18 +334,8 @@ int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p) {
 	hw->CRTC[20] = 0x00;
 	hw->CRTC[21] = vd /* & 0xFF */;
 	hw->CRTC[22] = (vt + 1) /* & 0xFF */;
-	if (text) {
-		if (ACCESS_FBINFO(devflags.textmode) == 1)
-			hw->CRTC[23] = 0xC3;
-		else
-			hw->CRTC[23] = 0xA3;
-		if (ACCESS_FBINFO(devflags.textmode) == 4)
-			hw->CRTC[20] = 0x5F;
-		else
-			hw->CRTC[20] = 0x1F;
-	} else
-		hw->CRTC[23] = 0xC3;
-	hw->CRTC[24] = 0xFF;
+	hw->CRTC[23] = 0xC3;
+	hw->CRTC[24] = lc;
 	return 0;
 };
 
@@ -426,7 +344,7 @@ void matroxfb_vgaHWrestore(WPMINFO2) {
 	struct matrox_hw_state * const hw = &ACCESS_FBINFO(hw);
 	CRITFLAGS
 
-	DBG("vgaHWrestore")
+	DBG(__FUNCTION__)
 
 	dprintk(KERN_INFO "MiscOutReg: %02X\n", hw->MiscOutReg);
 	dprintk(KERN_INFO "SEQ regs:   ");
@@ -473,182 +391,6 @@ void matroxfb_vgaHWrestore(WPMINFO2) {
 	CRITEND
 }
 
-void matroxfb_fastfont_init(struct matrox_fb_info* minfo){
-	unsigned int size;
-
-	size = ACCESS_FBINFO(fastfont.size);
-	ACCESS_FBINFO(fastfont.size) = 0;
-	if (size) {
-		unsigned int end = ACCESS_FBINFO(video.len_usable);
-
-		if (size < end) {
-			unsigned int start;
-
-			start = (end - size) & PAGE_MASK;
-			if (start >= 0x00100000) {
-				ACCESS_FBINFO(video.len_usable) = start;
-				ACCESS_FBINFO(fastfont.mgabase) = start * 8;
-				ACCESS_FBINFO(fastfont.vbase) = ACCESS_FBINFO(video.vbase);
-				vaddr_add(&ACCESS_FBINFO(fastfont.vbase), start);
-				ACCESS_FBINFO(fastfont.size) = end - start;
-			}
-		}
-	}
-}
-
-#ifndef FNTCHARCNT
-#define FNTCHARCNT(fd)	(((int *)(fd))[-3])
-#endif
-
-int matrox_text_loadfont(WPMINFO struct display* p) {
-	unsigned int fsize;
-	unsigned int width;
-	vaddr_t dst;
-	unsigned int i;
-	u_int8_t* font;
-	CRITFLAGS
-
-	if (!p || !p->fontdata)
-		return 0;
-	width = fontwidth(p);
-	fsize = p->userfont?FNTCHARCNT(p->fontdata):256;
-
-	dst = ACCESS_FBINFO(video.vbase);
-	i = 2;
-	font = (u_int8_t*)p->fontdata;
-
-	CRITBEGIN
-
-	mga_setr(M_SEQ_INDEX, 0x02, 0x04);
-	while (fsize--) {
-		int l;
-
-		for (l = 0; l < fontheight(p); l++) {
-			mga_writeb(dst, i, *font++);
-			if (fontwidth(p) > 8) font++;
-			i += ACCESS_FBINFO(devflags.vgastep);
-		}
-		i += (32 - fontheight(p)) * ACCESS_FBINFO(devflags.vgastep);
-	}
-	mga_setr(M_SEQ_INDEX, 0x02, 0x03);
-
-	CRITEND
-
-	return 1;
-}
-
-int matroxfb_fastfont_tryset(WPMINFO struct display* p) {
-	unsigned int fsize;
-	unsigned int width;
-	CRITFLAGS
-
-	if (!p || !p->fontdata)
-		return 0;
-	width = fontwidth(p);
-	if (width > 32)
-		return 0;
-	fsize = (p->userfont?FNTCHARCNT(p->fontdata):256) * fontheight(p);
-	if (((fsize * width + 31) / 32) * 4 > ACCESS_FBINFO(fastfont.size))
-		return 0;
-
-	CRITBEGIN
-
-	mga_outl(M_OPMODE, M_OPMODE_8BPP);
-	if (width <= 8) {
-		if (width == 8)
-			mga_memcpy_toio(ACCESS_FBINFO(fastfont.vbase), 0, p->fontdata, fsize);
-		else {
-			vaddr_t dst;
-			unsigned int i;
-			u_int8_t* font;
-			u_int32_t mask, valid, reg;
-
-			dst = ACCESS_FBINFO(fastfont.vbase);
-			font = (u_int8_t*)p->fontdata;
-			mask = ~0 << (8 - width);
-			valid = 0;
-			reg = 0;
-			i = 0;
-			while (fsize--) {
-				reg |= (*font++ & mask) << (8 - valid);
-				valid += width;
-				if (valid >= 8) {
-					mga_writeb(dst, i++, reg >> 8);
-					reg = reg << 8;
-					valid -= 8;
-				}
-			}
-			if (valid)
-				mga_writeb(dst, i, reg >> 8);
-		}
-	} else if (width <= 16) {
-		if (width == 16)
-			mga_memcpy_toio(ACCESS_FBINFO(fastfont.vbase), 0, p->fontdata, fsize*2);
-		else {
-			vaddr_t dst;
-			u_int16_t* font;
-			u_int32_t mask, valid, reg;
-			unsigned int i;
-
-			dst = ACCESS_FBINFO(fastfont.vbase);
-			font = (u_int16_t*)p->fontdata;
-			mask = ~0 << (16 - width);
-			valid = 0;
-			reg = 0;
-			i = 0;
-			while (fsize--) {
-				reg |= (ntohs(*font++) & mask) << (16 - valid);
-				valid += width;
-				if (valid >= 16) {
-					mga_writew(dst, i, htons(reg >> 16));
-					i += 2;
-					reg = reg << 16;
-					valid -= 16;
-				}
-			}
-			if (valid)
-				mga_writew(dst, i, htons(reg >> 16));
-		}
-	} else {
-		if (width == 32)
-			mga_memcpy_toio(ACCESS_FBINFO(fastfont.vbase), 0, p->fontdata, fsize*4);
-		else {
-			vaddr_t dst;
-			u_int32_t* font;
-			u_int32_t mask, valid, reg;
-			unsigned int i;
-
-			dst = ACCESS_FBINFO(fastfont.vbase);
-			font = (u_int32_t*)p->fontdata;
-			mask = ~0 << (32 - width);
-			valid = 0;
-			reg = 0;
-			i = 0;
-			while (fsize--) {
-				reg |= (ntohl(*font) & mask) >> valid;
-				valid += width;
-				if (valid >= 32) {
-					mga_writel(dst, i, htonl(reg));
-					i += 4;
-					valid -= 32;
-					if (valid)
-						reg = (ntohl(*font) & mask) << (width - valid);
-					else
-						reg = 0;
-				}
-				font++;
-			}
-			if (valid)
-				mga_writel(dst, i, htonl(reg));
-		}
-	}
-	mga_outl(M_OPMODE, ACCESS_FBINFO(accel.m_opmode));
-
-	CRITEND
-
-	return 1;
-}
-
 static void get_pins(unsigned char* pins, struct matrox_bios* bd) {
 	unsigned int b0 = readb(pins);
 	
@@ -994,6 +736,27 @@ void matroxfb_read_pins(WPMINFO2) {
 	parse_bios(vaddr_va(ACCESS_FBINFO(video).vbase), &ACCESS_FBINFO(bios));
 	pci_write_config_dword(pdev, PCI_ROM_ADDRESS, biosbase);
 	pci_write_config_dword(pdev, PCI_OPTION_REG, opt);
+#ifdef CONFIG_X86
+	if (!ACCESS_FBINFO(bios).bios_valid) {
+		unsigned char* b;
+
+		b = ioremap(0x000C0000, 65536);
+		if (!b) {
+			printk(KERN_INFO "matroxfb: Unable to map legacy BIOS\n");
+		} else {
+			unsigned int ven = readb(b+0x64+0) | (readb(b+0x64+1) << 8);
+			unsigned int dev = readb(b+0x64+2) | (readb(b+0x64+3) << 8);
+			
+			if (ven != pdev->vendor || dev != pdev->device) {
+				printk(KERN_INFO "matroxfb: Legacy BIOS is for %04X:%04X, while this device is %04X:%04X\n",
+					ven, dev, pdev->vendor, pdev->device);
+			} else {
+				parse_bios(b, &ACCESS_FBINFO(bios));
+			}
+			iounmap(b);
+		}
+	}
+#endif
 	matroxfb_set_limits(PMINFO &ACCESS_FBINFO(bios));
 }
 
@@ -1005,10 +768,6 @@ EXPORT_SYMBOL(matroxfb_PLL_calcclock);
 struct matrox_fb_info matroxfb_global_mxinfo;
 EXPORT_SYMBOL(matroxfb_global_mxinfo);
 #endif
-EXPORT_SYMBOL(matrox_text_loadfont);	/* for matroxfb_accel */
-EXPORT_SYMBOL(matroxfb_createcursorshape);	/* accel, DAC1064, Ti3026 */
-EXPORT_SYMBOL(matroxfb_fastfont_tryset);	/* accel */
-EXPORT_SYMBOL(matroxfb_fastfont_init);		/* DAC1064, Ti3026 */
 EXPORT_SYMBOL(matroxfb_vgaHWinit);		/* DAC1064, Ti3026 */
 EXPORT_SYMBOL(matroxfb_vgaHWrestore);		/* DAC1064, Ti3026 */
 EXPORT_SYMBOL(matroxfb_read_pins);

drivers/video/matrox/matroxfb_misc.h

@@ -11,12 +11,8 @@ static inline int PLL_calcclock(CPMINFO unsigned int freq, unsigned int fmax,
 	return matroxfb_PLL_calcclock(&ACCESS_FBINFO(features.pll), freq, fmax, in, feed, post);
 }
 
-void matroxfb_createcursorshape(WPMINFO struct display* p, int vmode);
-int matroxfb_vgaHWinit(WPMINFO struct my_timming* m, struct display* p);
+int matroxfb_vgaHWinit(WPMINFO struct my_timming* m);
 void matroxfb_vgaHWrestore(WPMINFO2);
-void matroxfb_fastfont_init(struct matrox_fb_info* minfo);
-int matrox_text_loadfont(WPMINFO struct display* p);
-int matroxfb_fastfont_tryset(WPMINFO struct display* p);
 void matroxfb_read_pins(WPMINFO2);
 
 #endif	/* __MATROXFB_MISC_H__ */

include/linux/matroxfb.h

@@ -3,6 +3,7 @@
 
 #include <asm/ioctl.h>
 #include <asm/types.h>
+#include <linux/videodev2.h>
 
 struct matroxioc_output_mode {
 	__u32	output;		/* which output */
@@ -30,5 +31,13 @@ struct matroxioc_output_mode {
 /* which outputs exist on this framebuffer */
 #define MATROXFB_GET_ALL_OUTPUTS	_IOR('n',0xFB,sizeof(__u32))
 
+enum matroxfb_ctrl_id {
+  MATROXFB_CID_TESTOUT	 = V4L2_CID_PRIVATE_BASE,
+  MATROXFB_CID_DEFLICKER,
+  MATROXFB_CID_LAST
+};
+
+#define FBIO_WAITFORVSYNC	_IOW('F', 0x20, u_int32_t)
+
 #endif
 

include/linux/xfrm.h

@@ -44,7 +44,7 @@ struct xfrm_selector
 	uid_t	user;
 };
 
-#define XFRM_INF (~(u64)0)
+#define XFRM_INF (~(__u64)0)
 
 struct xfrm_lifetime_cfg
 {
@@ -164,7 +164,7 @@ struct xfrm_usersa_info {
 };
 
 struct xfrm_usersa_id {
-	xfrm_address_t			saddr;
+	xfrm_address_t			daddr;
 	__u32				spi;
 	__u16				family;
 	__u8				proto;

include/net/sock.h

@@ -157,18 +157,18 @@ do {	spin_lock_init(&((__sk)->lock.slock)); \
  */
 struct sock {
 	/* Begin of struct sock/struct tcp_tw_bucket shared layout */
-	volatile unsigned char	state,
-				zapped;
+	unsigned short		family;
+	volatile unsigned char	state;
 	unsigned char		reuse;
-	unsigned char		shutdown;
 	int			bound_dev_if;
 	struct sock		*next;
 	struct sock		**pprev;
 	struct sock		*bind_next;
 	struct sock		**bind_pprev;
 	atomic_t		refcnt;
-	unsigned short		family;
 	/* End of struct sock/struct tcp_tw_bucket shared layout */
+	volatile unsigned char	zapped;
+	unsigned char		shutdown;
 	unsigned char		use_write_queue;
 	unsigned char		userlocks;
 	socket_lock_t		lock;

include/net/tcp.h

@@ -169,10 +169,9 @@ struct tcp_tw_bucket {
 	 * XXX Yes I know this is gross, but I'd have to edit every single
 	 * XXX networking file if I created a "struct sock_header". -DaveM
 	 */
-	volatile unsigned char	state,		/* Connection state	      */
-				substate;	/* "zapped" -> "substate"     */
+	unsigned short		family;
+	volatile unsigned char	state;		/* Connection state	      */
 	unsigned char		reuse;		/* SO_REUSEADDR setting       */
-	unsigned char		rcv_wscale;	/* also TW bucket specific    */
 	int			bound_dev_if;
 	/* Main hash linkage for various protocol lookup tables. */
 	struct sock		*next;
@@ -180,8 +179,9 @@ struct tcp_tw_bucket {
 	struct sock		*bind_next;
 	struct sock		**bind_pprev;
 	atomic_t		refcnt;
-	unsigned short		family;
 	/* End of struct sock/struct tcp_tw_bucket shared layout */
+	volatile unsigned char	substate;
+	unsigned char		rcv_wscale;
 	__u16			sport;
 	/* Socket demultiplex comparisons on incoming packets. */
 	/* these five are in inet_opt */

lib/zlib_deflate/deflate.c

@@ -255,10 +255,11 @@ int zlib_deflateInit2_(
 }
 
 /* ========================================================================= */
-int zlib_deflateSetDictionary (strm, dictionary, dictLength)
-    z_streamp strm;
-    const Bytef *dictionary;
-    uInt  dictLength;
+int zlib_deflateSetDictionary(
+	z_streamp strm,
+	const Bytef *dictionary,
+	uInt  dictLength
+)
 {
     deflate_state *s;
     uInt length = dictLength;
@@ -298,8 +299,9 @@ int zlib_deflateSetDictionary (strm, dictionary, dictLength)
 }
 
 /* ========================================================================= */
-int zlib_deflateReset (strm)
-    z_streamp strm;
+int zlib_deflateReset(
+	z_streamp strm
+)
 {
     deflate_state *s;
     
@@ -328,10 +330,11 @@ int zlib_deflateReset (strm)
 }
 
 /* ========================================================================= */
-int zlib_deflateParams(strm, level, strategy)
-    z_streamp strm;
-    int level;
-    int strategy;
+int zlib_deflateParams(
+	z_streamp strm,
+	int level,
+	int strategy
+)
 {
     deflate_state *s;
     compress_func func;
@@ -405,9 +408,10 @@ local void flush_pending(strm)
 }
 
 /* ========================================================================= */
-int zlib_deflate (strm, flush)
-    z_streamp strm;
-    int flush;
+int zlib_deflate(
+	z_streamp strm,
+	int flush
+)
 {
     int old_flush; /* value of flush param for previous deflate call */
     deflate_state *s;
@@ -542,8 +546,9 @@ int zlib_deflate (strm, flush)
 }
 
 /* ========================================================================= */
-int zlib_deflateEnd (strm)
-    z_streamp strm;
+int zlib_deflateEnd(
+	z_streamp strm
+)
 {
     int status;
     deflate_state *s;
@@ -565,9 +570,10 @@ int zlib_deflateEnd (strm)
 /* =========================================================================
  * Copy the source state to the destination state.
  */
-int zlib_deflateCopy (dest, source)
-    z_streamp dest;
-    z_streamp source;
+int zlib_deflateCopy (
+	z_streamp dest,
+	z_streamp source
+)
 {
 #ifdef MAXSEG_64K
     return Z_STREAM_ERROR;
@@ -624,10 +630,11 @@ int zlib_deflateCopy (dest, source)
  * allocating a large strm->next_in buffer and copying from it.
  * (See also flush_pending()).
  */
-local int read_buf(strm, buf, size)
-    z_streamp strm;
-    Bytef *buf;
-    unsigned size;
+local int read_buf(
+	z_streamp strm,
+	Bytef *buf,
+	unsigned size
+)
 {
     unsigned len = strm->avail_in;
 

net/xfrm/xfrm_policy.c

@@ -280,7 +280,6 @@ int xfrm_policy_insert(int dir, struct xfrm_policy *policy, int excl)
 	write_unlock_bh(&xfrm_policy_lock);
 
 	if (delpol) {
-		atomic_dec(&delpol->refcnt);
 		xfrm_policy_kill(delpol);
 		xfrm_pol_put(delpol);
 	}

net/xfrm/xfrm_state.c

@@ -112,9 +112,9 @@ static void xfrm_timer_handler(unsigned long data)
 		if (tmo < next)
 			next = tmo;
 	}
-	if (x->lft.hard_use_expires_seconds && x->curlft.use_time) {
+	if (x->lft.hard_use_expires_seconds) {
 		long tmo = x->lft.hard_use_expires_seconds +
-			x->curlft.use_time - now;
+			(x->curlft.use_time ? : now) - now;
 		if (tmo <= 0)
 			goto expired;
 		if (tmo < next)
@@ -130,9 +130,9 @@ static void xfrm_timer_handler(unsigned long data)
 		else if (tmo < next)
 			next = tmo;
 	}
-	if (x->lft.soft_use_expires_seconds && x->curlft.use_time) {
+	if (x->lft.soft_use_expires_seconds) {
 		long tmo = x->lft.soft_use_expires_seconds +
-			x->curlft.use_time - now;
+			(x->curlft.use_time ? : now) - now;
 		if (tmo <= 0)
 			warn = 1;
 		else if (tmo < next)

net/xfrm/xfrm_user.c

@@ -260,7 +260,7 @@ static int xfrm_add_sa(struct sk_buff *skb, struct nlmsghdr *nlh, void **xfrma)
 	if (!x)
 		return err;
 
-	x1 = xfrm_state_lookup(&x->props.saddr, x->id.spi, x->id.proto, x->props.family);
+	x1 = xfrm_state_lookup(&x->id.daddr, x->id.spi, x->id.proto, x->props.family);
 	if (x1) {
 		xfrm_state_put(x);
 		xfrm_state_put(x1);
@@ -277,7 +277,7 @@ static int xfrm_del_sa(struct sk_buff *skb, struct nlmsghdr *nlh, void **xfrma)
 	struct xfrm_state *x;
 	struct xfrm_usersa_id *p = NLMSG_DATA(nlh);
 
-	x = xfrm_state_lookup(&p->saddr, p->spi, p->proto, p->family);
+	x = xfrm_state_lookup(&p->daddr, p->spi, p->proto, p->family);
 	if (x == NULL)
 		return -ESRCH;
 
@@ -403,7 +403,7 @@ static int xfrm_get_sa(struct sk_buff *skb, struct nlmsghdr *nlh, void **xfrma)
 	struct sk_buff *resp_skb;
 	int err;
 
-	x = xfrm_state_lookup(&p->saddr, p->spi, p->proto, p->family);
+	x = xfrm_state_lookup(&p->daddr, p->spi, p->proto, p->family);
 	err = -ESRCH;
 	if (x == NULL)
 		goto out_noput;