1 files changed, 65 insertions, 9 deletions

diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index fc65c02306dd..8ffdf3578768 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -31,7 +31,8 @@
 #define ATMEL_LCDC_CVAL_DEFAULT         0xc8
 #define ATMEL_LCDC_DMA_BURST_LEN        8
 
-#if defined(CONFIG_ARCH_AT91SAM9263) || defined(CONFIG_ARCH_AT91CAP9)
+#if defined(CONFIG_ARCH_AT91SAM9263) || defined(CONFIG_ARCH_AT91CAP9) || \
+        defined(CONFIG_ARCH_AT91SAM9RL)
 #define ATMEL_LCDC_FIFO_SIZE            2048
 #else
 #define ATMEL_LCDC_FIFO_SIZE            512
@@ -250,6 +251,8 @@ static int atmel_lcdfb_alloc_video_memory(struct atmel_lcdfb_info *sinfo)
                 return -ENOMEM;
         }
 
+        memset(info->screen_base, 0, info->fix.smem_len);
+
         return 0;
 }
 
@@ -336,19 +339,35 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
                 break;
         case 15:
         case 16:
-                var->red.offset = 0;
+                if (sinfo->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
+                        /* RGB:565 mode */
+                        var->red.offset = 11;
+                        var->blue.offset = 0;
+                        var->green.length = 6;
+                } else {
+                        /* BGR:555 mode */
+                        var->red.offset = 0;
+                        var->blue.offset = 10;
+                        var->green.length = 5;
+                }
                 var->green.offset = 5;
-                var->blue.offset = 10;
-                var->red.length = var->green.length = var->blue.length = 5;
+                var->red.length = var->blue.length = 5;
                 break;
         case 32:
                 var->transp.offset = 24;
                 var->transp.length = 8;
                 /* fall through */
         case 24:
-                var->red.offset = 0;
+                if (sinfo->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
+                        /* RGB:888 mode */
+                        var->red.offset = 16;
+                        var->blue.offset = 0;
+                } else {
+                        /* BGR:888 mode */
+                        var->red.offset = 0;
+                        var->blue.offset = 16;
+                }
                 var->green.offset = 8;
-                var->blue.offset = 16;
                 var->red.length = var->green.length = var->blue.length = 8;
                 break;
         default:
@@ -634,7 +653,6 @@ static int __init atmel_lcdfb_init_fbinfo(struct atmel_lcdfb_info *sinfo)
         struct fb_info *info = sinfo->info;
         int ret = 0;
 
-        memset_io(info->screen_base, 0, info->fix.smem_len);
         info->var.activate |= FB_ACTIVATE_FORCE | FB_ACTIVATE_NOW;
 
         dev_info(info->device,
@@ -696,6 +714,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
                 sinfo->atmel_lcdfb_power_control = pdata_sinfo->atmel_lcdfb_power_control;
                 sinfo->guard_time = pdata_sinfo->guard_time;
                 sinfo->lcdcon_is_backlight = pdata_sinfo->lcdcon_is_backlight;
+                sinfo->lcd_wiring_mode = pdata_sinfo->lcd_wiring_mode;
         } else {
                 dev_err(dev, "cannot get default configuration\n");
                 goto free_info;
@@ -764,6 +783,11 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
                 info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
                 if (!info->screen_base)
                         goto release_intmem;
+
+                /*
+                 * Don't clear the framebuffer -- someone may have set
+                 * up a splash image.
+                 */
         } else {
                 /* alocate memory buffer */
                 ret = atmel_lcdfb_alloc_video_memory(sinfo);
@@ -903,10 +927,42 @@ static int __exit atmel_lcdfb_remove(struct platform_device *pdev)
         return 0;
 }
 
+#ifdef CONFIG_PM
+
+static int atmel_lcdfb_suspend(struct platform_device *pdev, pm_message_t mesg)
+{
+        struct fb_info *info = platform_get_drvdata(pdev);
+        struct atmel_lcdfb_info *sinfo = info->par;
+
+        sinfo->saved_lcdcon = lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_VAL);
+        lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, 0);
+        if (sinfo->atmel_lcdfb_power_control)
+                sinfo->atmel_lcdfb_power_control(0);
+        atmel_lcdfb_stop_clock(sinfo);
+        return 0;
+}
+
+static int atmel_lcdfb_resume(struct platform_device *pdev)
+{
+        struct fb_info *info = platform_get_drvdata(pdev);
+        struct atmel_lcdfb_info *sinfo = info->par;
+
+        atmel_lcdfb_start_clock(sinfo);
+        if (sinfo->atmel_lcdfb_power_control)
+                sinfo->atmel_lcdfb_power_control(1);
+        lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, sinfo->saved_lcdcon);
+        return 0;
+}
+
+#else
+#define atmel_lcdfb_suspend     NULL
+#define atmel_lcdfb_resume      NULL
+#endif
+
 static struct platform_driver atmel_lcdfb_driver = {
         .remove         = __exit_p(atmel_lcdfb_remove),
-
-// FIXME need suspend, resume
+        .suspend        = atmel_lcdfb_suspend,
+        .resume         = atmel_lcdfb_resume,
 
         .driver         = {
                 .name   = "atmel_lcdfb",