1 files changed, 52 insertions, 5 deletions

diff --git a/drivers/video/pxa168fb.c b/drivers/video/pxa168fb.c
index c91a7f7..cea6403 100644
--- a/drivers/video/pxa168fb.c
+++ b/drivers/video/pxa168fb.c
@@ -298,8 +298,8 @@ static void set_dma_control0(struct pxa168fb_info *fbi)
 	 * Set bit to enable graphics DMA.
 	 */
 	x = readl(fbi->reg_base + LCD_SPU_DMA_CTRL0);
-	x |= fbi->active ? 0x00000100 : 0;
-	fbi->active = 0;
+	x &= ~CFG_GRA_ENA_MASK;
+	x |= fbi->active ? CFG_GRA_ENA(1) : CFG_GRA_ENA(0);
 
 	/*
 	 * If we are in a pseudo-color mode, we need to enable
@@ -559,7 +559,7 @@ static struct fb_ops pxa168fb_ops = {
 	.fb_imageblit	= cfb_imageblit,
 };
 
-static int __init pxa168fb_init_mode(struct fb_info *info,
+static int __devinit pxa168fb_init_mode(struct fb_info *info,
 			      struct pxa168fb_mach_info *mi)
 {
 	struct pxa168fb_info *fbi = info->par;
@@ -599,7 +599,7 @@ static int __init pxa168fb_init_mode(struct fb_info *info,
 	return ret;
 }
 
-static int __init pxa168fb_probe(struct platform_device *pdev)
+static int __devinit pxa168fb_probe(struct platform_device *pdev)
 {
 	struct pxa168fb_mach_info *mi;
 	struct fb_info *info = 0;
@@ -784,20 +784,67 @@ failed:
 	return ret;
 }
 
+static int __devexit pxa168fb_remove(struct platform_device *pdev)
+{
+	struct pxa168fb_info *fbi = platform_get_drvdata(pdev);
+	struct fb_info *info;
+	int irq;
+	unsigned int data;
+
+	if (!fbi)
+		return 0;
+
+	/* disable DMA transfer */
+	data = readl(fbi->reg_base + LCD_SPU_DMA_CTRL0);
+	data &= ~CFG_GRA_ENA_MASK;
+	writel(data, fbi->reg_base + LCD_SPU_DMA_CTRL0);
+
+	info = fbi->info;
+
+	unregister_framebuffer(info);
+
+	writel(GRA_FRAME_IRQ0_ENA(0x0), fbi->reg_base + SPU_IRQ_ENA);
+
+	if (info->cmap.len)
+		fb_dealloc_cmap(&info->cmap);
+
+	irq = platform_get_irq(pdev, 0);
+	free_irq(irq, fbi);
+
+	dma_free_writecombine(fbi->dev, PAGE_ALIGN(info->fix.smem_len),
+				info->screen_base, info->fix.smem_start);
+
+	iounmap(fbi->reg_base);
+
+	clk_disable(fbi->clk);
+	clk_put(fbi->clk);
+
+	framebuffer_release(info);
+
+	return 0;
+}
+
 static struct platform_driver pxa168fb_driver = {
 	.driver		= {
 		.name	= "pxa168-fb",
 		.owner	= THIS_MODULE,
 	},
 	.probe		= pxa168fb_probe,
+	.remove		= __devexit_p(pxa168fb_remove),
 };
 
-static int __devinit pxa168fb_init(void)
+static int __init pxa168fb_init(void)
 {
 	return platform_driver_register(&pxa168fb_driver);
 }
 module_init(pxa168fb_init);
 
+static void __exit pxa168fb_exit(void)
+{
+	platform_driver_unregister(&pxa168fb_driver);
+}
+module_exit(pxa168fb_exit);
+
 MODULE_AUTHOR("Lennert Buytenhek <buytenh@marvell.com> "
 	      "Green Wan <gwan@marvell.com>");
 MODULE_DESCRIPTION("Framebuffer driver for PXA168/910");