1 files changed, 153 insertions, 35 deletions

diff --git a/drivers/video/pxafb.c b/drivers/video/pxafb.c
index 3ab6e3d973a1..69de2fed6c58 100644
--- a/drivers/video/pxafb.c
+++ b/drivers/video/pxafb.c
@@ -30,6 +30,7 @@
 #include <linux/string.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
+#include <linux/mm.h>
 #include <linux/fb.h>
 #include <linux/delay.h>
 #include <linux/init.h>
@@ -40,6 +41,7 @@
 #include <linux/clk.h>
 #include <linux/err.h>
 #include <linux/completion.h>
+#include <linux/mutex.h>
 #include <linux/kthread.h>
 #include <linux/freezer.h>
 
@@ -227,6 +229,22 @@ static int pxafb_bpp_to_lccr3(struct fb_var_screeninfo *var)
 	case 4:  ret = LCCR3_4BPP; break;
 	case 8:  ret = LCCR3_8BPP; break;
 	case 16: ret = LCCR3_16BPP; break;
+	case 24:
+		switch (var->red.length + var->green.length +
+				var->blue.length + var->transp.length) {
+		case 18: ret = LCCR3_18BPP_P | LCCR3_PDFOR_3; break;
+		case 19: ret = LCCR3_19BPP_P; break;
+		}
+		break;
+	case 32:
+		switch (var->red.length + var->green.length +
+				var->blue.length + var->transp.length) {
+		case 18: ret = LCCR3_18BPP | LCCR3_PDFOR_3; break;
+		case 19: ret = LCCR3_19BPP; break;
+		case 24: ret = LCCR3_24BPP | LCCR3_PDFOR_3; break;
+		case 25: ret = LCCR3_25BPP; break;
+		}
+		break;
 	}
 	return ret;
 }
@@ -345,6 +363,41 @@ static int pxafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 		var->green.offset = 5;  var->green.length = 6;
 		var->blue.offset  = 0;  var->blue.length  = 5;
 		var->transp.offset = var->transp.length = 0;
+	} else if (var->bits_per_pixel > 16) {
+		struct pxafb_mode_info *mode;
+
+		mode = pxafb_getmode(inf, var);
+		if (!mode)
+			return -EINVAL;
+
+		switch (mode->depth) {
+		case 18: /* RGB666 */
+			var->transp.offset = var->transp.length     = 0;
+			var->red.offset	   = 12; var->red.length    = 6;
+			var->green.offset  = 6;  var->green.length  = 6;
+			var->blue.offset   = 0;  var->blue.length   = 6;
+			break;
+		case 19: /* RGBT666 */
+			var->transp.offset = 18; var->transp.length = 1;
+			var->red.offset	   = 12; var->red.length    = 6;
+			var->green.offset  = 6;  var->green.length  = 6;
+			var->blue.offset   = 0;  var->blue.length   = 6;
+			break;
+		case 24: /* RGB888 */
+			var->transp.offset = var->transp.length     = 0;
+			var->red.offset	   = 16; var->red.length    = 8;
+			var->green.offset  = 8;  var->green.length  = 8;
+			var->blue.offset   = 0;  var->blue.length   = 8;
+			break;
+		case 25: /* RGBT888 */
+			var->transp.offset = 24; var->transp.length = 1;
+			var->red.offset	   = 16; var->red.length    = 8;
+			var->green.offset  = 8;  var->green.length  = 8;
+			var->blue.offset   = 0;  var->blue.length   = 8;
+			break;
+		default:
+			return -EINVAL;
+		}
 	} else {
 		var->red.offset = var->green.offset = 0;
 		var->blue.offset = var->transp.offset = 0;
@@ -355,9 +408,8 @@ static int pxafb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 	}
 
 #ifdef CONFIG_CPU_FREQ
-	pr_debug("pxafb: dma period = %d ps, clock = %d kHz\n",
-		 pxafb_display_dma_period(var),
-		 get_clk_frequency_khz(0));
+	pr_debug("pxafb: dma period = %d ps\n",
+		 pxafb_display_dma_period(var));
 #endif
 
 	return 0;
@@ -377,7 +429,7 @@ static int pxafb_set_par(struct fb_info *info)
 	struct pxafb_info *fbi = (struct pxafb_info *)info;
 	struct fb_var_screeninfo *var = &info->var;
 
-	if (var->bits_per_pixel == 16)
+	if (var->bits_per_pixel >= 16)
 		fbi->fb.fix.visual = FB_VISUAL_TRUECOLOR;
 	else if (!fbi->cmap_static)
 		fbi->fb.fix.visual = FB_VISUAL_PSEUDOCOLOR;
@@ -392,7 +444,7 @@ static int pxafb_set_par(struct fb_info *info)
 
 	fbi->fb.fix.line_length = var->xres_virtual *
 				  var->bits_per_pixel / 8;
-	if (var->bits_per_pixel == 16)
+	if (var->bits_per_pixel >= 16)
 		fbi->palette_size = 0;
 	else
 		fbi->palette_size = var->bits_per_pixel == 1 ?
@@ -405,7 +457,7 @@ static int pxafb_set_par(struct fb_info *info)
 	 */
 	pxafb_set_truecolor(fbi->fb.fix.visual == FB_VISUAL_TRUECOLOR);
 
-	if (fbi->fb.var.bits_per_pixel == 16)
+	if (fbi->fb.var.bits_per_pixel >= 16)
 		fb_dealloc_cmap(&fbi->fb.cmap);
 	else
 		fb_alloc_cmap(&fbi->fb.cmap, 1<<fbi->fb.var.bits_per_pixel, 0);
@@ -574,8 +626,8 @@ static int setup_frame_dma(struct pxafb_info *fbi, int dma, int pal,
 		dma_desc->fdadr = fbi->dma_buff_phys + dma_desc_off;
 		fbi->fdadr[dma] = fbi->dma_buff_phys + dma_desc_off;
 	} else {
-		pal_desc = &fbi->dma_buff->pal_desc[dma];
-		pal_desc_off = offsetof(struct pxafb_dma_buff, dma_desc[pal]);
+		pal_desc = &fbi->dma_buff->pal_desc[pal];
+		pal_desc_off = offsetof(struct pxafb_dma_buff, pal_desc[pal]);
 
 		pal_desc->fsadr = fbi->dma_buff_phys + pal * PALETTE_SIZE;
 		pal_desc->fidr  = 0;
@@ -832,6 +884,8 @@ static int pxafb_activate_var(struct fb_var_screeninfo *var,
 		case 4:
 		case 8:
 		case 16:
+		case 24:
+		case 32:
 			break;
 		default:
 			printk(KERN_ERR "%s: invalid bit depth %d\n",
@@ -969,6 +1023,11 @@ static void pxafb_setup_gpio(struct pxafb_info *fbi)
 
 	for (gpio = 58; ldd_bits; gpio++, ldd_bits--)
 		pxa_gpio_mode(gpio | GPIO_ALT_FN_2_OUT);
+	/* 18 bit interface */
+	if (fbi->fb.var.bits_per_pixel > 16) {
+		pxa_gpio_mode(86 | GPIO_ALT_FN_2_OUT);
+		pxa_gpio_mode(87 | GPIO_ALT_FN_2_OUT);
+	}
 	pxa_gpio_mode(GPIO74_LCD_FCLK_MD);
 	pxa_gpio_mode(GPIO75_LCD_LCLK_MD);
 	pxa_gpio_mode(GPIO76_LCD_PCLK_MD);
@@ -1059,7 +1118,7 @@ static void set_ctrlr_state(struct pxafb_info *fbi, u_int state)
 {
 	u_int old_state;
 
-	down(&fbi->ctrlr_sem);
+	mutex_lock(&fbi->ctrlr_lock);
 
 	old_state = fbi->state;
 
@@ -1147,7 +1206,7 @@ static void set_ctrlr_state(struct pxafb_info *fbi, u_int state)
 		}
 		break;
 	}
-	up(&fbi->ctrlr_sem);
+	mutex_unlock(&fbi->ctrlr_lock);
 }
 
 /*
@@ -1247,7 +1306,7 @@ static int pxafb_resume(struct platform_device *dev)
  *      cache.  Once this area is remapped, all virtual memory
  *      access to the video memory should occur at the new region.
  */
-static int __init pxafb_map_video_memory(struct pxafb_info *fbi)
+static int __devinit pxafb_map_video_memory(struct pxafb_info *fbi)
 {
 	/*
 	 * We reserve one page for the palette, plus the size
@@ -1277,6 +1336,8 @@ static int __init pxafb_map_video_memory(struct pxafb_info *fbi)
 		fbi->dma_buff_phys = fbi->map_dma;
 		fbi->palette_cpu = (u16 *) fbi->dma_buff->palette;
 
+	        pr_debug("pxafb: palette_mem_size = 0x%08x\n", fbi->palette_size*sizeof(u16));
+
 #ifdef CONFIG_FB_PXA_SMARTPANEL
 		fbi->smart_cmds = (uint16_t *) fbi->dma_buff->cmd_buff;
 		fbi->n_smart_cmds = 0;
@@ -1301,8 +1362,8 @@ static void pxafb_decode_mode_info(struct pxafb_info *fbi,
 	}
 }
 
-static int pxafb_decode_mach_info(struct pxafb_info *fbi,
-				  struct pxafb_mach_info *inf)
+static void pxafb_decode_mach_info(struct pxafb_info *fbi,
+				   struct pxafb_mach_info *inf)
 {
 	unsigned int lcd_conn = inf->lcd_conn;
 
@@ -1333,7 +1394,7 @@ static int pxafb_decode_mach_info(struct pxafb_info *fbi,
 		fbi->lccr0 = inf->lccr0;
 		fbi->lccr3 = inf->lccr3;
 		fbi->lccr4 = inf->lccr4;
-		return -EINVAL;
+		goto decode_mode;
 	}
 
 	if (lcd_conn == LCD_MONO_STN_8BPP)
@@ -1343,16 +1404,15 @@ static int pxafb_decode_mach_info(struct pxafb_info *fbi,
 	fbi->lccr3 |= (lcd_conn & LCD_BIAS_ACTIVE_LOW) ? LCCR3_OEP : 0;
 	fbi->lccr3 |= (lcd_conn & LCD_PCLK_EDGE_FALL)  ? LCCR3_PCP : 0;
 
+decode_mode:
 	pxafb_decode_mode_info(fbi, inf->modes, inf->num_modes);
-	return 0;
 }
 
-static struct pxafb_info * __init pxafb_init_fbinfo(struct device *dev)
+static struct pxafb_info * __devinit pxafb_init_fbinfo(struct device *dev)
 {
 	struct pxafb_info *fbi;
 	void *addr;
 	struct pxafb_mach_info *inf = dev->platform_data;
-	struct pxafb_mode_info *mode = inf->modes;
 
 	/* Alloc the pxafb_info and pseudo_palette in one step */
 	fbi = kmalloc(sizeof(struct pxafb_info) + sizeof(u32) * 16, GFP_KERNEL);
@@ -1399,7 +1459,7 @@ static struct pxafb_info * __init pxafb_init_fbinfo(struct device *dev)
 
 	init_waitqueue_head(&fbi->ctrlr_wait);
 	INIT_WORK(&fbi->task, pxafb_task);
-	init_MUTEX(&fbi->ctrlr_sem);
+	mutex_init(&fbi->ctrlr_lock);
 	init_completion(&fbi->disable_done);
 #ifdef CONFIG_FB_PXA_SMARTPANEL
 	init_completion(&fbi->command_done);
@@ -1410,7 +1470,7 @@ static struct pxafb_info * __init pxafb_init_fbinfo(struct device *dev)
 }
 
 #ifdef CONFIG_FB_PXA_PARAMETERS
-static int __init parse_opt_mode(struct device *dev, const char *this_opt)
+static int __devinit parse_opt_mode(struct device *dev, const char *this_opt)
 {
 	struct pxafb_mach_info *inf = dev->platform_data;
 
@@ -1469,7 +1529,7 @@ done:
 	return 0;
 }
 
-static int __init parse_opt(struct device *dev, char *this_opt)
+static int __devinit parse_opt(struct device *dev, char *this_opt)
 {
 	struct pxafb_mach_info *inf = dev->platform_data;
 	struct pxafb_mode_info *mode = &inf->modes[0];
@@ -1567,7 +1627,7 @@ static int __init parse_opt(struct device *dev, char *this_opt)
 	return 0;
 }
 
-static int __init pxafb_parse_options(struct device *dev, char *options)
+static int __devinit pxafb_parse_options(struct device *dev, char *options)
 {
 	char *this_opt;
 	int ret;
@@ -1588,8 +1648,8 @@ static int __init pxafb_parse_options(struct device *dev, char *options)
 
 static char g_options[256] __devinitdata = "";
 
-#ifndef CONFIG_MODULES
-static int __devinit pxafb_setup_options(void)
+#ifndef MODULE
+static int __init pxafb_setup_options(void)
 {
 	char *options = NULL;
 
@@ -1613,7 +1673,7 @@ MODULE_PARM_DESC(options, "LCD parameters (see Documentation/fb/pxafb.txt)");
 #define pxafb_setup_options()		(0)
 #endif
 
-static int __init pxafb_probe(struct platform_device *dev)
+static int __devinit pxafb_probe(struct platform_device *dev)
 {
 	struct pxafb_info *fbi;
 	struct pxafb_mach_info *inf;
@@ -1685,14 +1745,14 @@ static int __init pxafb_probe(struct platform_device *dev)
 	if (r == NULL) {
 		dev_err(&dev->dev, "no I/O memory resource defined\n");
 		ret = -ENODEV;
-		goto failed;
+		goto failed_fbi;
 	}
 
 	r = request_mem_region(r->start, r->end - r->start + 1, dev->name);
 	if (r == NULL) {
 		dev_err(&dev->dev, "failed to request I/O memory\n");
 		ret = -EBUSY;
-		goto failed;
+		goto failed_fbi;
 	}
 
 	fbi->mmio_base = ioremap(r->start, r->end - r->start + 1);
@@ -1735,8 +1795,17 @@ static int __init pxafb_probe(struct platform_device *dev)
 	 * This makes sure that our colour bitfield
 	 * descriptors are correctly initialised.
 	 */
-	pxafb_check_var(&fbi->fb.var, &fbi->fb);
-	pxafb_set_par(&fbi->fb);
+	ret = pxafb_check_var(&fbi->fb.var, &fbi->fb);
+	if (ret) {
+		dev_err(&dev->dev, "failed to get suitable mode\n");
+		goto failed_free_irq;
+	}
+
+	ret = pxafb_set_par(&fbi->fb);
+	if (ret) {
+		dev_err(&dev->dev, "Failed to set parameters\n");
+		goto failed_free_irq;
+	}
 
 	platform_set_drvdata(dev, fbi);
 
@@ -1744,7 +1813,7 @@ static int __init pxafb_probe(struct platform_device *dev)
 	if (ret < 0) {
 		dev_err(&dev->dev,
 			"Failed to register framebuffer device: %d\n", ret);
-		goto failed_free_irq;
+		goto failed_free_cmap;
 	}
 
 #ifdef CONFIG_CPU_FREQ
@@ -1763,31 +1832,74 @@ static int __init pxafb_probe(struct platform_device *dev)
 
 	return 0;
 
+failed_free_cmap:
+	if (fbi->fb.cmap.len)
+		fb_dealloc_cmap(&fbi->fb.cmap);
 failed_free_irq:
 	free_irq(irq, fbi);
-failed_free_res:
-	release_mem_region(r->start, r->end - r->start + 1);
-failed_free_io:
-	iounmap(fbi->mmio_base);
 failed_free_mem:
 	dma_free_writecombine(&dev->dev, fbi->map_size,
 			fbi->map_cpu, fbi->map_dma);
-failed:
+failed_free_io:
+	iounmap(fbi->mmio_base);
+failed_free_res:
+	release_mem_region(r->start, r->end - r->start + 1);
+failed_fbi:
+	clk_put(fbi->clk);
 	platform_set_drvdata(dev, NULL);
 	kfree(fbi);
+failed:
 	return ret;
 }
 
+static int __devexit pxafb_remove(struct platform_device *dev)
+{
+	struct pxafb_info *fbi = platform_get_drvdata(dev);
+	struct resource *r;
+	int irq;
+	struct fb_info *info;
+
+	if (!fbi)
+		return 0;
+
+	info = &fbi->fb;
+
+	unregister_framebuffer(info);
+
+	pxafb_disable_controller(fbi);
+
+	if (fbi->fb.cmap.len)
+		fb_dealloc_cmap(&fbi->fb.cmap);
+
+	irq = platform_get_irq(dev, 0);
+	free_irq(irq, fbi);
+
+	dma_free_writecombine(&dev->dev, fbi->map_size,
+					fbi->map_cpu, fbi->map_dma);
+
+	iounmap(fbi->mmio_base);
+
+	r = platform_get_resource(dev, IORESOURCE_MEM, 0);
+	release_mem_region(r->start, r->end - r->start + 1);
+
+	clk_put(fbi->clk);
+	kfree(fbi);
+
+	return 0;
+}
+
 static struct platform_driver pxafb_driver = {
 	.probe		= pxafb_probe,
+	.remove 	= pxafb_remove,
 	.suspend	= pxafb_suspend,
 	.resume		= pxafb_resume,
 	.driver		= {
+		.owner	= THIS_MODULE,
 		.name	= "pxa2xx-fb",
 	},
 };
 
-static int __devinit pxafb_init(void)
+static int __init pxafb_init(void)
 {
 	if (pxafb_setup_options())
 		return -EINVAL;
@@ -1795,7 +1907,13 @@ static int __devinit pxafb_init(void)
 	return platform_driver_register(&pxafb_driver);
 }
 
+static void __exit pxafb_exit(void)
+{
+	platform_driver_unregister(&pxafb_driver);
+}
+
 module_init(pxafb_init);
+module_exit(pxafb_exit);
 
 MODULE_DESCRIPTION("loadable framebuffer driver for PXA");
 MODULE_LICENSE("GPL");