[Gc-linux-cvs] linux/drivers/video gamecubefb.c,1.8,1.9

[<he...@us...>]

Update of /cvsroot/gc-linux/linux/drivers/video
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv8277

Modified Files:
	gamecubefb.c 
Log Message:
- Lindented source.
- We now try to free resources if something goes wrong.
- Changed the RGB to YCbYCr conversion code to make it more clear, specially
  because of the integer arithmetic tricks.
- Implemented a faster output to screen without memory penalties.
- Switched from RGB555 16bpp to RGB565 16bpp.
  This makes the console text white again, although not pure white because
  we still lack correct scaling for 5/6 bits to 8 bits r g b components.


Index: gamecubefb.c
===================================================================
RCS file: /cvsroot/gc-linux/linux/drivers/video/gamecubefb.c,v
retrieving revision 1.8
retrieving revision 1.9
diff -u -d -r1.8 -r1.9
--- gamecubefb.c	5 Feb 2004 13:45:17 -0000	1.8
+++ gamecubefb.c	8 Aug 2004 23:19:57 -0000	1.9
@@ -20,42 +20,45 @@
 #include <linux/init.h>
 #include <asm/io.h>
 
-/* --------------------------------------------------------------------- */
-volatile static unsigned int *gamecube_video = (unsigned int*) 0xCC002000;
+volatile static unsigned int *gamecube_video = (unsigned int *)0xCC002000;
+
 static const unsigned int VIDEO_Mode640X480NtscYUV16[32] = {
-0x0F060001, 0x476901AD, 0x02EA5140, 0x00030018,
-0x00020019, 0x410C410C, 0x40ED40ED, 0x00435A4E,
-0x00000000, 0x00435A4E, 0x00000000, 0x00000000,
-0x110701AE, 0x10010001, 0x00010001, 0x00010001,
-0x00000000, 0x00000000, 0x28500100, 0x1AE771F0,
-0x0DB4A574, 0x00C1188E, 0xC4C0CBE2, 0xFCECDECF,
-0x13130F08, 0x00080C0F, 0x00FF0000, 0x00000000,
-0x02800000, 0x000000FF, 0x00FF00FF, 0x00FF00FF};
+	0x0F060001, 0x476901AD, 0x02EA5140, 0x00030018,
+	0x00020019, 0x410C410C, 0x40ED40ED, 0x00435A4E,
+	0x00000000, 0x00435A4E, 0x00000000, 0x00000000,
+	0x110701AE, 0x10010001, 0x00010001, 0x00010001,
+	0x00000000, 0x00000000, 0x28500100, 0x1AE771F0,
+	0x0DB4A574, 0x00C1188E, 0xC4C0CBE2, 0xFCECDECF,
+	0x13130F08, 0x00080C0F, 0x00FF0000, 0x00000000,
+	0x02800000, 0x000000FF, 0x00FF00FF, 0x00FF00FF
+};
+
 static const unsigned int VIDEO_Mode640X480Pal50YUV16[32] = {
-0x11F50101, 0x4B6A01B0, 0x02F85640, 0x00010023,
-0x00000024, 0x4D2B4D6D, 0x4D8A4D4C, 0x00435A4E,
-0x00000000, 0x00435A4E, 0x00000000, 0x013C0144,
-0x113901B1, 0x10010001, 0x00010001, 0x00010001,
-0x00000000, 0x00000000, 0x28500100, 0x1AE771F0,
-0x0DB4A574, 0x00C1188E, 0xC4C0CBE2, 0xFCECDECF,
-0x13130F08, 0x00080C0F, 0x00FF0000, 0x00000000,
-0x02800000, 0x000000FF, 0x00FF00FF, 0x00FF00FF};
+	0x11F50101, 0x4B6A01B0, 0x02F85640, 0x00010023,
+	0x00000024, 0x4D2B4D6D, 0x4D8A4D4C, 0x00435A4E,
+	0x00000000, 0x00435A4E, 0x00000000, 0x013C0144,
+	0x113901B1, 0x10010001, 0x00010001, 0x00010001,
+	0x00000000, 0x00000000, 0x28500100, 0x1AE771F0,
+	0x0DB4A574, 0x00C1188E, 0xC4C0CBE2, 0xFCECDECF,
+	0x13130F08, 0x00080C0F, 0x00FF0000, 0x00000000,
+	0x02800000, 0x000000FF, 0x00FF00FF, 0x00FF00FF
+};
 
-static struct fb_var_screeninfo gamecubefb_defined __initdata = {
-	.activate	= FB_ACTIVATE_NOW,
-	.height		= -1,
-	.width		= -1,
-	.right_margin	= 32,
-	.upper_margin	= 16,
-	.lower_margin	= 4,
-	.vsync_len	= 4,
-	.vmode		= FB_VMODE_NONINTERLACED,
+static struct fb_var_screeninfo gcfb_defined __initdata = {
+	.activate = FB_ACTIVATE_NOW,
+	.height = -1,
+	.width = -1,
+	.right_margin = 32,
+	.upper_margin = 16,
+	.lower_margin = 4,
+	.vsync_len = 4,
+	.vmode = FB_VMODE_NONINTERLACED,
 };
 
-static struct fb_fix_screeninfo gamecubefb_fix __initdata = {
-	.id	= "GameCube",
-	.type	= FB_TYPE_PACKED_PIXELS,
-	.accel	= FB_ACCEL_NONE,
+static struct fb_fix_screeninfo gcfb_fix __initdata = {
+	.id = "GameCube",
+	.type = FB_TYPE_PACKED_PIXELS,
+	.accel = FB_ACCEL_NONE,
 };
 
 #define TV_ENC_DETECT 0
@@ -63,155 +66,113 @@
 #define TV_ENC_PAL 2
 static int tv_encoding __initdata = TV_ENC_DETECT;
 
-static struct fb_info fb_info;
+static struct fb_info gcfb_info;
 static u32 pseudo_palette[17];
 
-static int             ypan       = 0;  /* 0..nothing, 1..ypan, 2..ywrap */
-static unsigned short  *pmi_base  = 0;
-static void            (*pmi_start)(void);
-static void            (*pmi_pal)(void);
-
-
-/* --------------------------------------------------------------------- */
-// This is Costis' RGB to YCbYCr conversion code. This looks like a bad
-// hack, because we make the original cfbimgblit.c encode RGB 5:5:5 pixel
-// data and convert it into YCbYCr on every write of an int into the
-// framebuffer. But this is also quite good implementation, because YCbYCr
-// means that two pixels are always encoded together and, while each one
-// has its own luminance, they share the chrominance, so a putpixel() is
-// not possible, and just hooking into settwopixels() solves all problems.
-//
-// What doesn't work correctly right now, is setting a single pixel, because
-// the cfbimgblit.c code reads two pixels, assumes that it is RGB, changes
-// one pixel, and we'll convert it into YCbYCr. This breaks the encoding.
-// So we need to implement a 32 bit read from the framebuffer as well and
-// return RGB encoded data.
-#define CLAMP(x,l,h) ((x > h) ? h : ((x < l) ? l : x))
-
-// 16:16 fixed point... hopefully not much accuracy is lost!
-#define Ya 16843 // 0.257
-#define Yb 33030 // 0.504
-#define Yc 6423 // 0.098
-#define Yd 1048576 // 16.0
-#define Ye 32768 // 0.5
+static int ypan = 0;		/* 0..nothing, 1..ypan, 2..ywrap */
 
-#define Cba -9699 // 0.148
-#define Cbb -19071 // 0.291
-#define Cbc 28770 // 0.439
-#define Cbd 8388608 // 128.0
-#define Cbe 32768 // 0.5
+/*
+ * RGB to YCbYCr conversion support bits.
+ * We are using here the ITU.BT-601 Y'CbCr standard.
+ *
+ * References:
+ * - "Colour Space Conversions" by Adrian Ford and Alan Roberts, 1998
+ *   (google for coloureq.pdf)
+ *
+ */
 
-#define Cra 28770 // 0.439
-#define Crb -24117 // 0.368
-#define Crc -4653 // 0.071
-#define Crd 8388608 // 128.0
-#define Cre 32768 // 0.5
+#define RGB2YUV_SHIFT   16
+#define RGB2YUV_LUMA    16
+#define RGB2YUV_CHROMA 128
 
-#if 0
-unsigned long GC_Video_RGBToYCbCrFixed (unsigned char r, unsigned char g, unsigned char b)
-{
-	unsigned long Y, Cb, Cr;
+#define Yr ((int)( 0.299*(1<<RGB2YUV_SHIFT)))
+#define Yg ((int)( 0.587*(1<<RGB2YUV_SHIFT)))
+#define Yb ((int)( 0.114*(1<<RGB2YUV_SHIFT)))
 
-	Y = ((Ya * r) + (Yb * g) + (Yc * b) + Yd + Ye) >> 16;
-	Cb = ((Cba * r) + (Cbb * g) + (Cbc * b) + Cbd + Cre) >> 16;
-	Cr = ((Cra * r) + (Crb * g) + (Crc * b) + Crd + Cre) >> 16;
+#define Ur ((int)(-0.169*(1<<RGB2YUV_SHIFT)))
+#define Ug ((int)(-0.331*(1<<RGB2YUV_SHIFT)))
+#define Ub ((int)( 0.500*(1<<RGB2YUV_SHIFT)))
 
-    // clamping isnt really needed
-    Y  = CLAMP(Y , 16, 235);
-    Cb = CLAMP(Cb, 16, 240);
-    Cr = CLAMP(Cr, 16, 240);
+#define Vr ((int)( 0.500*(1<<RGB2YUV_SHIFT)))	/* same as Ub */
+#define Vg ((int)(-0.419*(1<<RGB2YUV_SHIFT)))
+#define Vb ((int)(-0.081*(1<<RGB2YUV_SHIFT)))
 
-	return (unsigned long)(((unsigned char)Y << 24) | ((unsigned char)Cb << 16) | ((unsigned char)Y << 8) | (unsigned char)Cr);
-}
+#define clamp(x, y, z) ((z < x) ? x : ((z > y) ? y : z))
 
-void gamecubefb_writel(unsigned long color, int *address)
+static inline uint32_t rgbrgb16toycbycr(uint16_t rgb1, uint16_t rgb2)
 {
-        unsigned char r, g, b;
-        unsigned long pa, pb;
-
-        r = ((color >> 27) & 31) << 3;
-        g = ((color >> 22) & 31) << 3;
-        b = ((color >> 17) & 31) << 3;
+	register int Y1, Cb, Y2, Cr;
+	register int r1, g1, b1;
+	register int r2, g2, b2;
+	register int r, g, b;
 
-        pa = GC_Video_RGBToYCbCrFixed (r, g, b);
+	/* fast path, thanks to bohdy */
+	if (!(rgb1 | rgb2)) {
+		return 0x00800080;	/* black, black */
+	}
 
-        r = ((color >> 11) & 31) << 3;
-        g = ((color >> 6) & 31) << 3;
-        b = ((color >> 1) & 31) << 3;
+	/*
+	 * FIXME:
+	 * The current scaling from 5 or 6 bits to 8 bits is not correct.
+	 */
 
-        pb = GC_Video_RGBToYCbCrFixed (r, g, b);
+	/* RGB565 */
+	r1 = ((rgb1 >> 11) & 0x1f) << 3;
+	g1 = ((rgb1 >> 5) & 0x3f) << 2;
+	b1 = ((rgb1 >> 0) & 0x1f) << 3;
 
-	fb_writel_real((pa & 0xFF000000) | (pb & 0x0000FF00) |
-		((((pa & 0x00FF0000) + (pb & 0x00FF0000)) >> 1) & 0x00FF0000) |
-		((((pa & 0x000000FF) + (pb & 0x000000FF)) >> 1) & 0x000000FF), address);
+	Y1 = clamp(16, 235, ((Yr * r1 + Yg * g1 + Yb * b1) >> RGB2YUV_SHIFT)
+		   + RGB2YUV_LUMA);
+	if (rgb1 == rgb2) {
+		/* this is just another fast path */
+		Y2 = Y1;
+		r = r1;
+		g = g1;
+		b = b1;
+	} else {
+		/* RGB565 */
+		r2 = ((rgb2 >> 11) & 0x1f) << 3;
+		g2 = ((rgb2 >> 5) & 0x3f) << 2;
+		b2 = ((rgb2 >> 0) & 0x1f) << 3;
 
-	//fb_writel_real((pa & 0xFF000000) | (pb & 0x0000FF00) |
-        //        (((pa & 0x00FF0000) + (pb & 0x00FF0000)) >> 1) |
-        //        (((pa & 0x000000FF) + (pb & 0x000000FF)) >> 1), address);
-}
-#endif
+		Y2 = clamp(16, 235,
+			   ((Yr * r2 + Yg * g2 + Yb * b2) >> RGB2YUV_SHIFT)
+			   + RGB2YUV_LUMA);
 
-/*
+		r = (r1 + r2) / 2;
+		g = (g1 + g2) / 2;
+		b = (b1 + b2) / 2;
+	}
 
-    this one is a bit cleaner ... still some potential for optimizations left
+	Cb = clamp(16, 240, ((Ur * r + Ug * g + Ub * b) >> RGB2YUV_SHIFT)
+		   + RGB2YUV_CHROMA);
+	Cr = clamp(16, 240, ((Vr * r + Vg * g + Vb * b) >> RGB2YUV_SHIFT)
+		   + RGB2YUV_CHROMA);
 
-*/
+	return (((uint8_t) Y1) << 24) | (((uint8_t) Cb) << 16) |
+	    (((uint8_t) Y2) << 8) | (((uint8_t) Cr) << 0);
+}
 
-void gamecubefb_writel(register unsigned long color, register int *address)
+void gamecubefb_writel(register unsigned long rgbrgb, register int *address)
 {
-        register unsigned char r1, g1, b1;
-        register unsigned char r2, g2, b2;
-        register unsigned char r, g, b;
-        register unsigned char Y1, Cb, Y2, Cr;
-
-        // mmh...arent we are loosing a bit much precision here? maybe
-        // a 5:6:5 format would be better ?
-        r1 = ((color >> 27) & 31) << 3;
-        g1 = ((color >> 22) & 31) << 3;
-        b1 = ((color >> 17) & 31) << 3;
-
-        r2 = ((color >> 11) & 31) << 3;
-        g2 = ((color >> 6) & 31) << 3;
-        b2 = ((color >> 1) & 31) << 3;
-
-        r=(((int)r1)+((int)r2))>>1;
-        g=(((int)g1)+((int)g2))>>1;
-        b=(((int)b1)+((int)b2))>>1;
-
-	Y1 = (((Ya * r1) + (Yb * g1) + (Yc * b1) + Yd + Ye) >> 16);
-	Y2 = (((Ya * r2) + (Yb * g2) + (Yc * b2) + Yd + Ye) >> 16);
-
-	Cb = (((Cba * r) + (Cbb * g) + (Cbc * b) + Cbd + Cre) >> 16);
-	Cr = (((Cra * r) + (Crb * g) + (Crc * b) + Crd + Cre) >> 16);
-
-    // clamping isnt really needed, so we omit it... previous calculations
-    // should be adjusted so we dont get out-of-gamut values
-/*
-    Y1 = CLAMP(Y , 16, 235);
-    Y2 = CLAMP(Y , 16, 235);
-    Cb = CLAMP(Cb, 16, 240);
-    Cr = CLAMP(Cr, 16, 240);
-*/
-    fb_writel_real(((((int)Y1)<<24)|(((int)Cb)<<16)|(((int)Y2)<<8)|(((int)Cr)<<0)),address);
-
+	uint16_t *rgb = (uint16_t *) & rgbrgb;
+	fb_writel_real(rgbrgb16toycbycr(rgb[0], rgb[1]), address);
 }
 
-
-/* --------------------------------------------------------------------- */
-
 static int gamecubefb_pan_display(struct fb_var_screeninfo *var,
-                              struct fb_info *info)
+				  struct fb_info *info)
 {
 	return 0;
 }
 
-static void vesa_setpalette(int regno, unsigned red, unsigned green, unsigned blue)
+static void vesa_setpalette(int regno, unsigned red, unsigned green,
+			    unsigned blue)
 {
 }
 
 static int gamecubefb_setcolreg(unsigned regno, unsigned red, unsigned green,
-			    unsigned blue, unsigned transp,
-			    struct fb_info *info)
+				unsigned blue, unsigned transp,
+				struct fb_info *info)
 {
 	/*
 	 *  Set a single color register. The values supplied are
@@ -225,67 +186,46 @@
 
 	switch (info->var.bits_per_pixel) {
 	case 8:
-		vesa_setpalette(regno,red,green,blue);
+		vesa_setpalette(regno, red, green, blue);
 		break;
+	case 15:
 	case 16:
 		if (info->var.red.offset == 10) {
+			/* XXX, not used currently */
 			/* 1:5:5:5 */
-			((u32*) (info->pseudo_palette))[regno] =
-					((red   & 0xf800) >>  1) |
-					((green & 0xf800) >>  6) |
-					((blue  & 0xf800) >> 11);
+			((u32 *) (info->pseudo_palette))[regno] =
+			    ((red & 0xf800) >> 1) |
+			    ((green & 0xf800) >> 6) | ((blue & 0xf800) >> 11);
 		} else {
 			/* 0:5:6:5 */
-			((u32*) (info->pseudo_palette))[regno] =
-					((red   & 0xf800)      ) |
-					((green & 0xfc00) >>  5) |
-					((blue  & 0xf800) >> 11);
+			((u32 *) (info->pseudo_palette))[regno] =
+			    ((red & 0xf800)) |
+			    ((green & 0xfc00) >> 5) | ((blue & 0xf800) >> 11);
 		}
 		break;
 	case 24:
-		red   >>= 8;
-		green >>= 8;
-		blue  >>= 8;
-		((u32 *)(info->pseudo_palette))[regno] =
-			(red   << info->var.red.offset)   |
-			(green << info->var.green.offset) |
-			(blue  << info->var.blue.offset);
-		break;
 	case 32:
-		red   >>= 8;
+		/* XXX, not used currently */
+		red >>= 8;
 		green >>= 8;
-		blue  >>= 8;
-		((u32 *)(info->pseudo_palette))[regno] =
-			(red   << info->var.red.offset)   |
-			(green << info->var.green.offset) |
-			(blue  << info->var.blue.offset);
+		blue >>= 8;
+		((u32 *) (info->pseudo_palette))[regno] =
+		    (red << info->var.red.offset) |
+		    (green << info->var.green.offset) |
+		    (blue << info->var.blue.offset);
 		break;
-    }
-    return 0;
-}
-
-void gc_blit() {
-	printk("BLIT\n\n\n");
+	}
+	return 0;
 }
 
 static struct fb_ops gamecubefb_ops = {
-	.owner		= THIS_MODULE,
-#if 0
-	.fb_setcolreg	= gc_blit,
-	.fb_pan_display	= gc_blit,
-	.fb_fillrect	= gc_blit,
-	.fb_copyarea	= gc_blit,
-	.fb_imageblit	= gc_blit,
-	.fb_cursor	= gc_blit,
-#endif
-#if 1
-	.fb_setcolreg	= gamecubefb_setcolreg,
-	.fb_pan_display	= gamecubefb_pan_display,
-	.fb_fillrect	= cfb_fillrect,
-	.fb_copyarea	= cfb_copyarea,
-	.fb_imageblit	= cfb_imageblit,
-	.fb_cursor	= soft_cursor,
-#endif
+	.owner = THIS_MODULE,
+	.fb_setcolreg = gamecubefb_setcolreg,
+	.fb_pan_display = gamecubefb_pan_display,
+	.fb_fillrect = cfb_fillrect,
+	.fb_copyarea = cfb_copyarea,
+	.fb_imageblit = cfb_imageblit,
+	.fb_cursor = soft_cursor,
 };
 
 int __init gamecubefb_setup(char *options)
@@ -297,21 +237,22 @@
 		return 0;
 
 	while ((this_opt = strsep(&options, ",")) != NULL) {
-	printk("this_opt = %s\n", this_opt);
-		if (!*this_opt) continue;
+		printk("this_opt = %s\n", this_opt);
+		if (!*this_opt)
+			continue;
 
-		if (! strcmp(this_opt, "redraw"))
-			ypan=0;
-		else if (! strcmp(this_opt, "ypan"))
-			ypan=1;
-		else if (! strcmp(this_opt, "ywrap"))
-			ypan=2;
+		if (!strcmp(this_opt, "redraw"))
+			ypan = 0;
+		else if (!strcmp(this_opt, "ypan"))
+			ypan = 1;
+		else if (!strcmp(this_opt, "ywrap"))
+			ypan = 2;
 		else if (!strncmp(this_opt, "tv=", 3)) {
-		printk("detected \"tv=\"\n");
-		printk("cmd line: %s\n", this_opt);
-			if(!strncmp(this_opt + 3, "PAL", 3))
+			printk("detected \"tv=\"\n");
+			printk("cmd line: %s\n", this_opt);
+			if (!strncmp(this_opt + 3, "PAL", 3))
 				tv_encoding = TV_ENC_PAL;
-			else if(!strncmp(this_opt + 3, "NTSC", 4))
+			else if (!strncmp(this_opt + 3, "NTSC", 4))
 				tv_encoding = TV_ENC_NTSC;
 		}
 	}
@@ -322,108 +263,155 @@
 {
 	int video_cmap_len;
 	int i;
+	int err = 0;
 
 	// detect current video mode
 	if (tv_encoding == TV_ENC_DETECT) {
 		tv_encoding = ((gamecube_video[0] >> 8) & 3) + 1;
 	}
 
-	gamecubefb_defined.bits_per_pixel = 16;
-	gamecubefb_defined.xres = 640;
-	gamecubefb_defined.yres = (tv_encoding == TV_ENC_NTSC) ? 480 : 576;
-	gamecubefb_fix.line_length = gamecubefb_defined.xres * (gamecubefb_defined.bits_per_pixel/8);
-	gamecubefb_fix.smem_len = gamecubefb_fix.line_length * gamecubefb_defined.yres;
-	gamecubefb_fix.smem_start = (24*1024*1024)-gamecubefb_fix.smem_len; /* end of RAM */
-	gamecubefb_fix.visual   = (gamecubefb_defined.bits_per_pixel == 8) ?
-		FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
+	gcfb_defined.bits_per_pixel = 16;
+	gcfb_defined.xres = 640;
+	gcfb_defined.yres = (tv_encoding == TV_ENC_NTSC) ? 480 : 576;
 
-	if (!request_mem_region(gamecubefb_fix.smem_start, gamecubefb_fix.smem_len, "gamecubefb")) {
+	gcfb_fix.line_length =
+	    gcfb_defined.xres * (gcfb_defined.bits_per_pixel / 8);
+	gcfb_fix.smem_len = gcfb_fix.line_length * gcfb_defined.yres;
+	/* place XFB at end of RAM */
+	gcfb_fix.smem_start = (24 * 1024 * 1024) - gcfb_fix.smem_len;
+
+	gcfb_fix.visual = (gcfb_defined.bits_per_pixel == 8) ?
+	    FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
+
+	if (!request_mem_region
+	    (gcfb_fix.smem_start, gcfb_fix.smem_len, "gamecubefb")) {
 		printk(KERN_WARNING
-		       "gamecubefb: abort, cannot reserve video memory at 0x%lx\n",
-			gamecubefb_fix.smem_start);
+		       "gamecubefb: abort, cannot reserve video memory at %p\n",
+		       (void *)gcfb_fix.smem_start);
 		/* We cannot make this fatal. Sometimes this comes from magic
 		   spaces our resource handlers simply don't know about */
 	}
 
-        fb_info.screen_base = ioremap(gamecubefb_fix.smem_start, gamecubefb_fix.smem_len);
-	if (!fb_info.screen_base) {
-		release_mem_region(gamecubefb_fix.smem_start, gamecubefb_fix.smem_len);
+	gcfb_info.screen_base = ioremap(gcfb_fix.smem_start, gcfb_fix.smem_len);
+	if (!gcfb_info.screen_base) {
 		printk(KERN_ERR
-		       "gamecubefb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
-			gamecubefb_fix.smem_len, gamecubefb_fix.smem_start);
-		return -EIO;
+		       "gamecubefb: abort, cannot ioremap video memory"
+		       " at %p (%dk)\n",
+		       (void *)gcfb_fix.smem_start, gcfb_fix.smem_len / 1024);
+		err = -EIO;
+		goto err_ioremap;
 	}
 
-	printk(KERN_INFO "gamecubefb: framebuffer at 0x%lx, mapped to 0x%p, size %dk\n",
-	       gamecubefb_fix.smem_start, fb_info.screen_base, gamecubefb_fix.smem_len/1024);
-	printk(KERN_INFO "gamecubefb: mode is %dx%dx%d, linelength=%d, pages=%d\n",
-	       gamecubefb_defined.xres, gamecubefb_defined.yres, gamecubefb_defined.bits_per_pixel, gamecubefb_fix.line_length, 0); //screen_info.pages);
+	printk(KERN_INFO
+	       "gamecubefb: framebuffer at 0x%p, mapped to 0x%p, size %dk\n",
+	       (void *)gcfb_fix.smem_start, gcfb_info.screen_base,
+	       gcfb_fix.smem_len / 1024);
+	printk(KERN_INFO
+	       "gamecubefb: mode is %dx%dx%d, linelength=%d, pages=%d\n",
+	       gcfb_defined.xres, gcfb_defined.yres,
+	       gcfb_defined.bits_per_pixel, gcfb_fix.line_length,
+	       0 /*screen_info.pages */ );
 
-	gamecubefb_defined.xres_virtual = gamecubefb_defined.xres;
-	gamecubefb_defined.yres_virtual = gamecubefb_defined.yres;
+	gcfb_defined.xres_virtual = gcfb_defined.xres;
+	gcfb_defined.yres_virtual = gcfb_defined.yres;
 	ypan = 0;
 
+	/* FIXME! Please, use here *real* values */
 	/* some dummy values for timing to make fbset happy */
-	gamecubefb_defined.pixclock     = 10000000 / gamecubefb_defined.xres * 1000 / gamecubefb_defined.yres;
-	gamecubefb_defined.left_margin  = (gamecubefb_defined.xres / 8) & 0xf8;
-	gamecubefb_defined.hsync_len    = (gamecubefb_defined.xres / 8) & 0xf8;
+	gcfb_defined.pixclock =
+	    10000000 / gcfb_defined.xres * 1000 / gcfb_defined.yres;
+	gcfb_defined.left_margin = (gcfb_defined.xres / 8) & 0xf8;
+	gcfb_defined.hsync_len = (gcfb_defined.xres / 8) & 0xf8;
 
-	if (gamecubefb_defined.bits_per_pixel > 8) {
-		gamecubefb_defined.red.offset    = 11;
-		gamecubefb_defined.red.length    = 5;
-		gamecubefb_defined.green.offset  = 6;
-		gamecubefb_defined.green.length  = 5;
-		gamecubefb_defined.blue.offset   = 1;
-		gamecubefb_defined.blue.length   = 5;
-		gamecubefb_defined.transp.offset = 15;
-		gamecubefb_defined.transp.length = 1;
+	if (gcfb_defined.bits_per_pixel == 15) {
+		gcfb_defined.red.offset = 11;
+		gcfb_defined.red.length = 5;
+		gcfb_defined.green.offset = 6;
+		gcfb_defined.green.length = 5;
+		gcfb_defined.blue.offset = 1;
+		gcfb_defined.blue.length = 5;
+		gcfb_defined.transp.offset = 15;
+		gcfb_defined.transp.length = 1;
+		video_cmap_len = 16;
+	} else if (gcfb_defined.bits_per_pixel == 16) {
+		gcfb_defined.red.offset = 11;
+		gcfb_defined.red.length = 5;
+		gcfb_defined.green.offset = 5;
+		gcfb_defined.green.length = 6;
+		gcfb_defined.blue.offset = 0;
+		gcfb_defined.blue.length = 5;
+		gcfb_defined.transp.offset = 0;
+		gcfb_defined.transp.length = 0;
 		video_cmap_len = 16;
 	} else {
-		gamecubefb_defined.red.length   = 6;
-		gamecubefb_defined.green.length = 6;
-		gamecubefb_defined.blue.length  = 6;
+		gcfb_defined.red.length = 6;
+		gcfb_defined.green.length = 6;
+		gcfb_defined.blue.length = 6;
 		video_cmap_len = 256;
 	}
 
-	gamecubefb_fix.ypanstep  = ypan     ? 1 : 0;
-	gamecubefb_fix.ywrapstep = (ypan>1) ? 1 : 0;
+	gcfb_fix.ypanstep = ypan ? 1 : 0;
+	gcfb_fix.ywrapstep = (ypan > 1) ? 1 : 0;
 
-	fb_info.fbops = &gamecubefb_ops;
-	fb_info.var = gamecubefb_defined;
-	fb_info.fix = gamecubefb_fix;
-	fb_info.pseudo_palette = pseudo_palette;
-	fb_info.flags = FBINFO_FLAG_DEFAULT;
+	gcfb_info.fbops = &gamecubefb_ops;
+	gcfb_info.var = gcfb_defined;
+	gcfb_info.fix = gcfb_fix;
+	gcfb_info.pseudo_palette = pseudo_palette;
+	gcfb_info.flags = FBINFO_FLAG_DEFAULT;
 
-	fb_alloc_cmap(&fb_info.cmap, video_cmap_len, 0);
+	if (fb_alloc_cmap(&gcfb_info.cmap, video_cmap_len, 0)) {
+		err = -ENOMEM;
+		goto err_alloc_cmap;
+	}
 
-	if (register_framebuffer(&fb_info)<0)
-		return -EINVAL;
+	if (register_framebuffer(&gcfb_info) < 0) {
+		err = -EINVAL;
+		goto err_register_framebuffer;
+	}
 
-	unsigned int *VIDEO_Mode;
+	/* fill framebuffer memory with black color */
+	int c = gcfb_defined.xres * gcfb_defined.yres / 2;
+	volatile unsigned long *p = (unsigned long *)gcfb_info.screen_base;
+	while (c--)
+		writel(0x00800080, p++);
 
-//	printk("tv_encoding = %i\n", tv_encoding);
+	unsigned int *VIDEO_Mode;
 
 	if (tv_encoding == TV_ENC_NTSC)
-		VIDEO_Mode = VIDEO_Mode640X480NtscYUV16;
+		VIDEO_Mode = (unsigned int *)VIDEO_Mode640X480NtscYUV16;
 	else
-		VIDEO_Mode = VIDEO_Mode640X480Pal50YUV16;
+		VIDEO_Mode = (unsigned int *)VIDEO_Mode640X480Pal50YUV16;
 
-	// initialize screen
-	for(i=0; i<32; i++) {
+	/* initialize video registers */
+	for (i = 0; i < 7; i++) {
+		gamecube_video[i] = VIDEO_Mode[i];
+	}
+	gamecube_video[8] = VIDEO_Mode[8];
+	for (i = 10; i < 32; i++) {
 		gamecube_video[i] = VIDEO_Mode[i];
 	}
 
-    gamecube_video[7] = 0x10000000 | (gamecubefb_fix.smem_start>>5);
+	gamecube_video[7] = 0x10000000 | (gcfb_fix.smem_start >> 5);
 
-    // setting both fields to same source means half the resolution, but
-    // reduces flickering a lot ...mmmh maybe worth a try as a last resort :/
-    // gamecube_video[9] = 0x10000000 | (gamecubefb_fix.smem_start>>5);
+// setting both fields to same source means half the resolution, but
+// reduces flickering a lot ...mmmh maybe worth a try as a last resort :/
+// gamecube_video[9] = 0x10000000 | (gcfb_fix.smem_start>>5);
 
-    gamecube_video[9] = 0x10000000 | ((gamecubefb_fix.smem_start+gamecubefb_fix.line_length)>>5);
+	gamecube_video[9] =
+	    0x10000000 | ((gcfb_fix.smem_start + gcfb_fix.line_length) >> 5);
 
 	printk(KERN_INFO "fb%d: %s frame buffer device\n",
-	       fb_info.node, fb_info.fix.id);
-	return 0;
+	       gcfb_info.node, gcfb_info.fix.id);
+	goto out;
+
+err_register_framebuffer:
+	fb_dealloc_cmap(&gcfb_info.cmap);
+err_alloc_cmap:
+	iounmap(gcfb_info.screen_base);
+err_ioremap:
+	release_mem_region(gcfb_fix.smem_start, gcfb_fix.smem_len);
+out:
+	return err;
 }
 
 /*