1 files changed, 138 insertions, 27 deletions

diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index b004036d4087..75dac578104f 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -18,9 +18,9 @@
 #include <linux/delay.h>
 #include <linux/backlight.h>
 
-#include <asm/arch/board.h>
-#include <asm/arch/cpu.h>
-#include <asm/arch/gpio.h>
+#include <mach/board.h>
+#include <mach/cpu.h>
+#include <mach/gpio.h>
 
 #include <video/atmel_lcdc.h>
 
@@ -39,7 +39,9 @@
 #endif
 
 #if defined(CONFIG_ARCH_AT91)
-#define ATMEL_LCDFB_FBINFO_DEFAULT      FBINFO_DEFAULT
+#define ATMEL_LCDFB_FBINFO_DEFAULT      (FBINFO_DEFAULT \
+                                         | FBINFO_PARTIAL_PAN_OK \
+                                         | FBINFO_HWACCEL_YPAN)
 
 static inline void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
                                         struct fb_var_screeninfo *var)
@@ -177,7 +179,7 @@ static struct fb_fix_screeninfo atmel_lcdfb_fix __initdata = {
         .type           = FB_TYPE_PACKED_PIXELS,
         .visual         = FB_VISUAL_TRUECOLOR,
         .xpanstep       = 0,
-        .ypanstep       = 0,
+        .ypanstep       = 1,
         .ywrapstep      = 0,
         .accel          = FB_ACCEL_NONE,
 };
@@ -206,6 +208,36 @@ static unsigned long compute_hozval(unsigned long xres, unsigned long lcdcon2)
         return value;
 }
 
+static void atmel_lcdfb_stop_nowait(struct atmel_lcdfb_info *sinfo)
+{
+        /* Turn off the LCD controller and the DMA controller */
+        lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
+                        sinfo->guard_time << ATMEL_LCDC_GUARDT_OFFSET);
+
+        /* Wait for the LCDC core to become idle */
+        while (lcdc_readl(sinfo, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
+                msleep(10);
+
+        lcdc_writel(sinfo, ATMEL_LCDC_DMACON, 0);
+}
+
+static void atmel_lcdfb_stop(struct atmel_lcdfb_info *sinfo)
+{
+        atmel_lcdfb_stop_nowait(sinfo);
+
+        /* Wait for DMA engine to become idle... */
+        while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
+                msleep(10);
+}
+
+static void atmel_lcdfb_start(struct atmel_lcdfb_info *sinfo)
+{
+        lcdc_writel(sinfo, ATMEL_LCDC_DMACON, sinfo->default_dmacon);
+        lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
+                (sinfo->guard_time << ATMEL_LCDC_GUARDT_OFFSET)
+                | ATMEL_LCDC_PWR);
+}
+
 static void atmel_lcdfb_update_dma(struct fb_info *info,
                                struct fb_var_screeninfo *var)
 {
@@ -240,9 +272,11 @@ static int atmel_lcdfb_alloc_video_memory(struct atmel_lcdfb_info *sinfo)
 {
         struct fb_info *info = sinfo->info;
         struct fb_var_screeninfo *var = &info->var;
+        unsigned int smem_len;
 
-        info->fix.smem_len = (var->xres_virtual * var->yres_virtual
-                            * ((var->bits_per_pixel + 7) / 8));
+        smem_len = (var->xres_virtual * var->yres_virtual
+                    * ((var->bits_per_pixel + 7) / 8));
+        info->fix.smem_len = max(smem_len, sinfo->smem_len);
 
         info->screen_base = dma_alloc_writecombine(info->device, info->fix.smem_len,
                                         (dma_addr_t *)&info->fix.smem_start, GFP_KERNEL);
@@ -256,6 +290,20 @@ static int atmel_lcdfb_alloc_video_memory(struct atmel_lcdfb_info *sinfo)
         return 0;
 }
 
+static const struct fb_videomode *atmel_lcdfb_choose_mode(struct fb_var_screeninfo *var,
+                                                     struct fb_info *info)
+{
+        struct fb_videomode varfbmode;
+        const struct fb_videomode *fbmode = NULL;
+
+        fb_var_to_videomode(&varfbmode, var);
+        fbmode = fb_find_nearest_mode(&varfbmode, &info->modelist);
+        if (fbmode)
+                fb_videomode_to_var(var, fbmode);
+        return fbmode;
+}
+
+
 /**
  *      atmel_lcdfb_check_var - Validates a var passed in.
  *      @var: frame buffer variable screen structure
@@ -289,6 +337,15 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
         clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;
 
         dev_dbg(dev, "%s:\n", __func__);
+
+        if (!(var->pixclock && var->bits_per_pixel)) {
+                /* choose a suitable mode if possible */
+                if (!atmel_lcdfb_choose_mode(var, info)) {
+                        dev_err(dev, "needed value not specified\n");
+                        return -EINVAL;
+                }
+        }
+
         dev_dbg(dev, "  resolution: %ux%u\n", var->xres, var->yres);
         dev_dbg(dev, "  pixclk:     %lu KHz\n", PICOS2KHZ(var->pixclock));
         dev_dbg(dev, "  bpp:        %u\n", var->bits_per_pixel);
@@ -299,6 +356,13 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
                 return -EINVAL;
         }
 
+        /* Do not allow to have real resoulution larger than virtual */
+        if (var->xres > var->xres_virtual)
+                var->xres_virtual = var->xres;
+
+        if (var->yres > var->yres_virtual)
+                var->yres_virtual = var->yres;
+
         /* Force same alignment for each line */
         var->xres = (var->xres + 3) & ~3UL;
         var->xres_virtual = (var->xres_virtual + 3) & ~3UL;
@@ -379,6 +443,17 @@ static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
         return 0;
 }
 
+/*
+ * LCD reset sequence
+ */
+static void atmel_lcdfb_reset(struct atmel_lcdfb_info *sinfo)
+{
+        might_sleep();
+
+        atmel_lcdfb_stop(sinfo);
+        atmel_lcdfb_start(sinfo);
+}
+
 /**
  *      atmel_lcdfb_set_par - Alters the hardware state.
  *      @info: frame buffer structure that represents a single frame buffer
@@ -401,19 +476,14 @@ static int atmel_lcdfb_set_par(struct fb_info *info)
         unsigned long clk_value_khz;
         unsigned long bits_per_line;
 
+        might_sleep();
+
         dev_dbg(info->device, "%s:\n", __func__);
         dev_dbg(info->device, "  * resolution: %ux%u (%ux%u virtual)\n",
                  info->var.xres, info->var.yres,
                  info->var.xres_virtual, info->var.yres_virtual);
 
-        /* Turn off the LCD controller and the DMA controller */
-        lcdc_writel(sinfo, ATMEL_LCDC_PWRCON, sinfo->guard_time << ATMEL_LCDC_GUARDT_OFFSET);
-
-        /* Wait for the LCDC core to become idle */
-        while (lcdc_readl(sinfo, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
-                msleep(10);
-
-        lcdc_writel(sinfo, ATMEL_LCDC_DMACON, 0);
+        atmel_lcdfb_stop_nowait(sinfo);
 
         if (info->var.bits_per_pixel == 1)
                 info->fix.visual = FB_VISUAL_MONO01;
@@ -511,18 +581,14 @@ static int atmel_lcdfb_set_par(struct fb_info *info)
 
         /* Disable all interrupts */
         lcdc_writel(sinfo, ATMEL_LCDC_IDR, ~0UL);
+        /* Enable FIFO & DMA errors */
+        lcdc_writel(sinfo, ATMEL_LCDC_IER, ATMEL_LCDC_UFLWI | ATMEL_LCDC_OWRI | ATMEL_LCDC_MERI);
 
         /* ...wait for DMA engine to become idle... */
         while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
                 msleep(10);
 
-        dev_dbg(info->device, "  * re-enable DMA engine\n");
-        /* ...and enable it with updated configuration */
-        lcdc_writel(sinfo, ATMEL_LCDC_DMACON, sinfo->default_dmacon);
-
-        dev_dbg(info->device, "  * re-enable LCDC core\n");
-        lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
-                (sinfo->guard_time << ATMEL_LCDC_GUARDT_OFFSET) | ATMEL_LCDC_PWR);
+        atmel_lcdfb_start(sinfo);
 
         dev_dbg(info->device, "  * DONE\n");
 
@@ -645,10 +711,26 @@ static irqreturn_t atmel_lcdfb_interrupt(int irq, void *dev_id)
         u32 status;
 
         status = lcdc_readl(sinfo, ATMEL_LCDC_ISR);
-        lcdc_writel(sinfo, ATMEL_LCDC_IDR, status);
+        if (status & ATMEL_LCDC_UFLWI) {
+                dev_warn(info->device, "FIFO underflow %#x\n", status);
+                /* reset DMA and FIFO to avoid screen shifting */
+                schedule_work(&sinfo->task);
+        }
+        lcdc_writel(sinfo, ATMEL_LCDC_ICR, status);
         return IRQ_HANDLED;
 }
 
+/*
+ * LCD controller task (to reset the LCD)
+ */
+static void atmel_lcdfb_task(struct work_struct *work)
+{
+        struct atmel_lcdfb_info *sinfo =
+                container_of(work, struct atmel_lcdfb_info, task);
+
+        atmel_lcdfb_reset(sinfo);
+}
+
 static int __init atmel_lcdfb_init_fbinfo(struct atmel_lcdfb_info *sinfo)
 {
         struct fb_info *info = sinfo->info;
@@ -691,6 +773,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
         struct fb_info *info;
         struct atmel_lcdfb_info *sinfo;
         struct atmel_lcdfb_info *pdata_sinfo;
+        struct fb_videomode fbmode;
         struct resource *regs = NULL;
         struct resource *map = NULL;
         int ret;
@@ -714,6 +797,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
                 sinfo->default_monspecs = pdata_sinfo->default_monspecs;
                 sinfo->atmel_lcdfb_power_control = pdata_sinfo->atmel_lcdfb_power_control;
                 sinfo->guard_time = pdata_sinfo->guard_time;
+                sinfo->smem_len = pdata_sinfo->smem_len;
                 sinfo->lcdcon_is_backlight = pdata_sinfo->lcdcon_is_backlight;
                 sinfo->lcd_wiring_mode = pdata_sinfo->lcd_wiring_mode;
         } else {
@@ -824,6 +908,10 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
                 goto unmap_mmio;
         }
 
+        /* Some operations on the LCDC might sleep and
+         * require a preemptible task context */
+        INIT_WORK(&sinfo->task, atmel_lcdfb_task);
+
         ret = atmel_lcdfb_init_fbinfo(sinfo);
         if (ret < 0) {
                 dev_err(dev, "init fbinfo failed: %d\n", ret);
@@ -850,9 +938,13 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
         ret = register_framebuffer(info);
         if (ret < 0) {
                 dev_err(dev, "failed to register framebuffer device: %d\n", ret);
-                goto free_cmap;
+                goto reset_drvdata;
         }
 
+        /* add selected videomode to modelist */
+        fb_var_to_videomode(&fbmode, &info->var);
+        fb_add_videomode(&fbmode, &info->modelist);
+
         /* Power up the LCDC screen */
         if (sinfo->atmel_lcdfb_power_control)
                 sinfo->atmel_lcdfb_power_control(1);
@@ -862,10 +954,12 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
 
         return 0;
 
-
+reset_drvdata:
+        dev_set_drvdata(dev, NULL);
 free_cmap:
         fb_dealloc_cmap(&info->cmap);
 unregister_irqs:
+        cancel_work_sync(&sinfo->task);
         free_irq(sinfo->irq_base, info);
 unmap_mmio:
         exit_backlight(sinfo);
@@ -898,11 +992,13 @@ static int __exit atmel_lcdfb_remove(struct platform_device *pdev)
 {
         struct device *dev = &pdev->dev;
         struct fb_info *info = dev_get_drvdata(dev);
-        struct atmel_lcdfb_info *sinfo = info->par;
+        struct atmel_lcdfb_info *sinfo;
 
-        if (!sinfo)
+        if (!info || !info->par)
                 return 0;
+        sinfo = info->par;
 
+        cancel_work_sync(&sinfo->task);
         exit_backlight(sinfo);
         if (sinfo->atmel_lcdfb_power_control)
                 sinfo->atmel_lcdfb_power_control(0);
@@ -935,11 +1031,20 @@ 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;
 
+        /*
+         * We don't want to handle interrupts while the clock is
+         * stopped. It may take forever.
+         */
+        lcdc_writel(sinfo, ATMEL_LCDC_IDR, ~0UL);
+
         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(sinfo);
         atmel_lcdfb_stop_clock(sinfo);
+
         return 0;
 }
 
@@ -949,9 +1054,15 @@ static int atmel_lcdfb_resume(struct platform_device *pdev)
         struct atmel_lcdfb_info *sinfo = info->par;
 
         atmel_lcdfb_start_clock(sinfo);
+        atmel_lcdfb_start(sinfo);
         if (sinfo->atmel_lcdfb_power_control)
                 sinfo->atmel_lcdfb_power_control(1);
         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, sinfo->saved_lcdcon);
+
+        /* Enable FIFO & DMA errors */
+        lcdc_writel(sinfo, ATMEL_LCDC_IER, ATMEL_LCDC_UFLWI
+                        | ATMEL_LCDC_OWRI | ATMEL_LCDC_MERI);
+
         return 0;
 }