drivers/video/fbdev/atmel_lcdfb.c

   1 /*
   2  *  Driver for AT91/AT32 LCD Controller
   3  *
   4  *  Copyright (C) 2007 Atmel Corporation
   5  *
   6  * This file is subject to the terms and conditions of the GNU General Public
   7  * License.  See the file COPYING in the main directory of this archive for
   8  * more details.
   9  */
  10
  11 #include <linux/kernel.h>
  12 #include <linux/platform_device.h>
  13 #include <linux/dma-mapping.h>
  14 #include <linux/interrupt.h>
  15 #include <linux/clk.h>
  16 #include <linux/fb.h>
  17 #include <linux/init.h>
  18 #include <linux/delay.h>
  19 #include <linux/backlight.h>
  20 #include <linux/gfp.h>
  21 #include <linux/gpio/consumer.h>
  22 #include <linux/module.h>
  23 #include <linux/of.h>
  24 #include <linux/of_device.h>
  25 #include <video/of_display_timing.h>
  26 #include <linux/regulator/consumer.h>
  27 #include <video/videomode.h>
  28
  29 #include <video/atmel_lcdc.h>
  30
  31 struct atmel_lcdfb_config {
  32         bool have_alt_pixclock;
  33         bool have_hozval;
  34         bool have_intensity_bit;
  35 };
  36
  37  /* LCD Controller info data structure, stored in device platform_data */
  38 struct atmel_lcdfb_info {
  39         spinlock_t              lock;
  40         struct fb_info          *info;
  41         void __iomem            *mmio;
  42         int                     irq_base;
  43         struct work_struct      task;
  44
  45         unsigned int            smem_len;
  46         struct platform_device  *pdev;
  47         struct clk              *bus_clk;
  48         struct clk              *lcdc_clk;
  49
  50         struct backlight_device *backlight;
  51         u8                      bl_power;
  52         u8                      saved_lcdcon;
  53
  54         u32                     pseudo_palette[16];
  55         bool                    have_intensity_bit;
  56
  57         struct atmel_lcdfb_pdata pdata;
  58
  59         struct atmel_lcdfb_config *config;
  60         struct regulator        *reg_lcd;
  61 };
  62
  63 struct atmel_lcdfb_power_ctrl_gpio {
  64         struct gpio_desc *gpiod;
  65
  66         struct list_head list;
  67 };
  68
  69 #define lcdc_readl(sinfo, reg)          __raw_readl((sinfo)->mmio+(reg))
  70 #define lcdc_writel(sinfo, reg, val)    __raw_writel((val), (sinfo)->mmio+(reg))
  71
  72 /* configurable parameters */
  73 #define ATMEL_LCDC_CVAL_DEFAULT         0xc8
  74 #define ATMEL_LCDC_DMA_BURST_LEN        8       /* words */
  75 #define ATMEL_LCDC_FIFO_SIZE            512     /* words */
  76
  77 static struct atmel_lcdfb_config at91sam9261_config = {
  78         .have_hozval            = true,
  79         .have_intensity_bit     = true,
  80 };
  81
  82 static struct atmel_lcdfb_config at91sam9263_config = {
  83         .have_intensity_bit     = true,
  84 };
  85
  86 static struct atmel_lcdfb_config at91sam9g10_config = {
  87         .have_hozval            = true,
  88 };
  89
  90 static struct atmel_lcdfb_config at91sam9g45_config = {
  91         .have_alt_pixclock      = true,
  92 };
  93
  94 static struct atmel_lcdfb_config at91sam9g45es_config = {
  95 };
  96
  97 static struct atmel_lcdfb_config at91sam9rl_config = {
  98         .have_intensity_bit     = true,
  99 };
 100
 101 static struct atmel_lcdfb_config at32ap_config = {
 102         .have_hozval            = true,
 103 };
 104
 105 static const struct platform_device_id atmel_lcdfb_devtypes[] = {
 106         {
 107                 .name = "at91sam9261-lcdfb",
 108                 .driver_data = (unsigned long)&at91sam9261_config,
 109         }, {
 110                 .name = "at91sam9263-lcdfb",
 111                 .driver_data = (unsigned long)&at91sam9263_config,
 112         }, {
 113                 .name = "at91sam9g10-lcdfb",
 114                 .driver_data = (unsigned long)&at91sam9g10_config,
 115         }, {
 116                 .name = "at91sam9g45-lcdfb",
 117                 .driver_data = (unsigned long)&at91sam9g45_config,
 118         }, {
 119                 .name = "at91sam9g45es-lcdfb",
 120                 .driver_data = (unsigned long)&at91sam9g45es_config,
 121         }, {
 122                 .name = "at91sam9rl-lcdfb",
 123                 .driver_data = (unsigned long)&at91sam9rl_config,
 124         }, {
 125                 .name = "at32ap-lcdfb",
 126                 .driver_data = (unsigned long)&at32ap_config,
 127         }, {
 128                 /* terminator */
 129         }
 130 };
 131 MODULE_DEVICE_TABLE(platform, atmel_lcdfb_devtypes);
 132
 133 static struct atmel_lcdfb_config *
 134 atmel_lcdfb_get_config(struct platform_device *pdev)
 135 {
 136         unsigned long data;
 137
 138         data = platform_get_device_id(pdev)->driver_data;
 139
 140         return (struct atmel_lcdfb_config *)data;
 141 }
 142
 143 #if defined(CONFIG_ARCH_AT91)
 144 #define ATMEL_LCDFB_FBINFO_DEFAULT      (FBINFO_DEFAULT \
 145                                          | FBINFO_PARTIAL_PAN_OK \
 146                                          | FBINFO_HWACCEL_YPAN)
 147
 148 static inline void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
 149                                         struct fb_var_screeninfo *var,
 150                                         struct fb_info *info)
 151 {
 152
 153 }
 154 #elif defined(CONFIG_AVR32)
 155 #define ATMEL_LCDFB_FBINFO_DEFAULT      (FBINFO_DEFAULT \
 156                                         | FBINFO_PARTIAL_PAN_OK \
 157                                         | FBINFO_HWACCEL_XPAN \
 158                                         | FBINFO_HWACCEL_YPAN)
 159
 160 static void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
 161                                      struct fb_var_screeninfo *var,
 162                                      struct fb_info *info)
 163 {
 164         u32 dma2dcfg;
 165         u32 pixeloff;
 166
 167         pixeloff = (var->xoffset * info->var.bits_per_pixel) & 0x1f;
 168
 169         dma2dcfg = (info->var.xres_virtual - info->var.xres)
 170                  * info->var.bits_per_pixel / 8;
 171         dma2dcfg |= pixeloff << ATMEL_LCDC_PIXELOFF_OFFSET;
 172         lcdc_writel(sinfo, ATMEL_LCDC_DMA2DCFG, dma2dcfg);
 173
 174         /* Update configuration */
 175         lcdc_writel(sinfo, ATMEL_LCDC_DMACON,
 176                     lcdc_readl(sinfo, ATMEL_LCDC_DMACON)
 177                     | ATMEL_LCDC_DMAUPDT);
 178 }
 179 #endif
 180
 181 static u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
 182                 | ATMEL_LCDC_POL_POSITIVE
 183                 | ATMEL_LCDC_ENA_PWMENABLE;
 184
 185 #ifdef CONFIG_BACKLIGHT_ATMEL_LCDC
 186
 187 /* some bl->props field just changed */
 188 static int atmel_bl_update_status(struct backlight_device *bl)
 189 {
 190         struct atmel_lcdfb_info *sinfo = bl_get_data(bl);
 191         int                     power = sinfo->bl_power;
 192         int                     brightness = bl->props.brightness;
 193
 194         /* REVISIT there may be a meaningful difference between
 195          * fb_blank and power ... there seem to be some cases
 196          * this doesn't handle correctly.
 197          */
 198         if (bl->props.fb_blank != sinfo->bl_power)
 199                 power = bl->props.fb_blank;
 200         else if (bl->props.power != sinfo->bl_power)
 201                 power = bl->props.power;
 202
 203         if (brightness < 0 && power == FB_BLANK_UNBLANK)
 204                 brightness = lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_VAL);
 205         else if (power != FB_BLANK_UNBLANK)
 206                 brightness = 0;
 207
 208         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, brightness);
 209         if (contrast_ctr & ATMEL_LCDC_POL_POSITIVE)
 210                 lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR,
 211                         brightness ? contrast_ctr : 0);
 212         else
 213                 lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
 214
 215         bl->props.fb_blank = bl->props.power = sinfo->bl_power = power;
 216
 217         return 0;
 218 }
 219
 220 static int atmel_bl_get_brightness(struct backlight_device *bl)
 221 {
 222         struct atmel_lcdfb_info *sinfo = bl_get_data(bl);
 223
 224         return lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_VAL);
 225 }
 226
 227 static const struct backlight_ops atmel_lcdc_bl_ops = {
 228         .update_status = atmel_bl_update_status,
 229         .get_brightness = atmel_bl_get_brightness,
 230 };
 231
 232 static void init_backlight(struct atmel_lcdfb_info *sinfo)
 233 {
 234         struct backlight_properties props;
 235         struct backlight_device *bl;
 236
 237         sinfo->bl_power = FB_BLANK_UNBLANK;
 238
 239         if (sinfo->backlight)
 240                 return;
 241
 242         memset(&props, 0, sizeof(struct backlight_properties));
 243         props.type = BACKLIGHT_RAW;
 244         props.max_brightness = 0xff;
 245         bl = backlight_device_register("backlight", &sinfo->pdev->dev, sinfo,
 246                                        &atmel_lcdc_bl_ops, &props);
 247         if (IS_ERR(bl)) {
 248                 dev_err(&sinfo->pdev->dev, "error %ld on backlight register\n",
 249                                 PTR_ERR(bl));
 250                 return;
 251         }
 252         sinfo->backlight = bl;
 253
 254         bl->props.power = FB_BLANK_UNBLANK;
 255         bl->props.fb_blank = FB_BLANK_UNBLANK;
 256         bl->props.brightness = atmel_bl_get_brightness(bl);
 257 }
 258
 259 static void exit_backlight(struct atmel_lcdfb_info *sinfo)
 260 {
 261         if (!sinfo->backlight)
 262                 return;
 263
 264         if (sinfo->backlight->ops) {
 265                 sinfo->backlight->props.power = FB_BLANK_POWERDOWN;
 266                 sinfo->backlight->ops->update_status(sinfo->backlight);
 267         }
 268         backlight_device_unregister(sinfo->backlight);
 269 }
 270
 271 #else
 272
 273 static void init_backlight(struct atmel_lcdfb_info *sinfo)
 274 {
 275         dev_warn(&sinfo->pdev->dev, "backlight control is not available\n");
 276 }
 277
 278 static void exit_backlight(struct atmel_lcdfb_info *sinfo)
 279 {
 280 }
 281
 282 #endif
 283
 284 static void init_contrast(struct atmel_lcdfb_info *sinfo)
 285 {
 286         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 287
 288         /* contrast pwm can be 'inverted' */
 289         if (pdata->lcdcon_pol_negative)
 290                 contrast_ctr &= ~(ATMEL_LCDC_POL_POSITIVE);
 291
 292         /* have some default contrast/backlight settings */
 293         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
 294         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, ATMEL_LCDC_CVAL_DEFAULT);
 295
 296         if (pdata->lcdcon_is_backlight)
 297                 init_backlight(sinfo);
 298 }
 299
 300 static inline void atmel_lcdfb_power_control(struct atmel_lcdfb_info *sinfo, int on)
 301 {
 302         int ret;
 303         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 304
 305         if (pdata->atmel_lcdfb_power_control)
 306                 pdata->atmel_lcdfb_power_control(pdata, on);
 307         else if (sinfo->reg_lcd) {
 308                 if (on) {
 309                         ret = regulator_enable(sinfo->reg_lcd);
 310                         if (ret)
 311                                 dev_err(&sinfo->pdev->dev,
 312                                         "lcd regulator enable failed:   %d\n", ret);
 313                 } else {
 314                         ret = regulator_disable(sinfo->reg_lcd);
 315                         if (ret)
 316                                 dev_err(&sinfo->pdev->dev,
 317                                         "lcd regulator disable failed: %d\n", ret);
 318                 }
 319         }
 320 }
 321
 322 static const struct fb_fix_screeninfo atmel_lcdfb_fix __initconst = {
 323         .type           = FB_TYPE_PACKED_PIXELS,
 324         .visual         = FB_VISUAL_TRUECOLOR,
 325         .xpanstep       = 0,
 326         .ypanstep       = 1,
 327         .ywrapstep      = 0,
 328         .accel          = FB_ACCEL_NONE,
 329 };
 330
 331 static unsigned long compute_hozval(struct atmel_lcdfb_info *sinfo,
 332                                                         unsigned long xres)
 333 {
 334         unsigned long lcdcon2;
 335         unsigned long value;
 336
 337         if (!sinfo->config->have_hozval)
 338                 return xres;
 339
 340         lcdcon2 = lcdc_readl(sinfo, ATMEL_LCDC_LCDCON2);
 341         value = xres;
 342         if ((lcdcon2 & ATMEL_LCDC_DISTYPE) != ATMEL_LCDC_DISTYPE_TFT) {
 343                 /* STN display */
 344                 if ((lcdcon2 & ATMEL_LCDC_DISTYPE) == ATMEL_LCDC_DISTYPE_STNCOLOR) {
 345                         value *= 3;
 346                 }
 347                 if ( (lcdcon2 & ATMEL_LCDC_IFWIDTH) == ATMEL_LCDC_IFWIDTH_4
 348                    || ( (lcdcon2 & ATMEL_LCDC_IFWIDTH) == ATMEL_LCDC_IFWIDTH_8
 349                       && (lcdcon2 & ATMEL_LCDC_SCANMOD) == ATMEL_LCDC_SCANMOD_DUAL ))
 350                         value = DIV_ROUND_UP(value, 4);
 351                 else
 352                         value = DIV_ROUND_UP(value, 8);
 353         }
 354
 355         return value;
 356 }
 357
 358 static void atmel_lcdfb_stop_nowait(struct atmel_lcdfb_info *sinfo)
 359 {
 360         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 361
 362         /* Turn off the LCD controller and the DMA controller */
 363         lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
 364                         pdata->guard_time << ATMEL_LCDC_GUARDT_OFFSET);
 365
 366         /* Wait for the LCDC core to become idle */
 367         while (lcdc_readl(sinfo, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
 368                 msleep(10);
 369
 370         lcdc_writel(sinfo, ATMEL_LCDC_DMACON, 0);
 371 }
 372
 373 static void atmel_lcdfb_stop(struct atmel_lcdfb_info *sinfo)
 374 {
 375         atmel_lcdfb_stop_nowait(sinfo);
 376
 377         /* Wait for DMA engine to become idle... */
 378         while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
 379                 msleep(10);
 380 }
 381
 382 static void atmel_lcdfb_start(struct atmel_lcdfb_info *sinfo)
 383 {
 384         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 385
 386         lcdc_writel(sinfo, ATMEL_LCDC_DMACON, pdata->default_dmacon);
 387         lcdc_writel(sinfo, ATMEL_LCDC_PWRCON,
 388                 (pdata->guard_time << ATMEL_LCDC_GUARDT_OFFSET)
 389                 | ATMEL_LCDC_PWR);
 390 }
 391
 392 static void atmel_lcdfb_update_dma(struct fb_info *info,
 393                                struct fb_var_screeninfo *var)
 394 {
 395         struct atmel_lcdfb_info *sinfo = info->par;
 396         struct fb_fix_screeninfo *fix = &info->fix;
 397         unsigned long dma_addr;
 398
 399         dma_addr = (fix->smem_start + var->yoffset * fix->line_length
 400                     + var->xoffset * info->var.bits_per_pixel / 8);
 401
 402         dma_addr &= ~3UL;
 403
 404         /* Set framebuffer DMA base address and pixel offset */
 405         lcdc_writel(sinfo, ATMEL_LCDC_DMABADDR1, dma_addr);
 406
 407         atmel_lcdfb_update_dma2d(sinfo, var, info);
 408 }
 409
 410 static inline void atmel_lcdfb_free_video_memory(struct atmel_lcdfb_info *sinfo)
 411 {
 412         struct fb_info *info = sinfo->info;
 413
 414         dma_free_wc(info->device, info->fix.smem_len, info->screen_base,
 415                     info->fix.smem_start);
 416 }
 417
 418 /**
 419  *      atmel_lcdfb_alloc_video_memory - Allocate framebuffer memory
 420  *      @sinfo: the frame buffer to allocate memory for
 421  *
 422  *      This function is called only from the atmel_lcdfb_probe()
 423  *      so no locking by fb_info->mm_lock around smem_len setting is needed.
 424  */
 425 static int atmel_lcdfb_alloc_video_memory(struct atmel_lcdfb_info *sinfo)
 426 {
 427         struct fb_info *info = sinfo->info;
 428         struct fb_var_screeninfo *var = &info->var;
 429         unsigned int smem_len;
 430
 431         smem_len = (var->xres_virtual * var->yres_virtual
 432                     * ((var->bits_per_pixel + 7) / 8));
 433         info->fix.smem_len = max(smem_len, sinfo->smem_len);
 434
 435         info->screen_base = dma_alloc_wc(info->device, info->fix.smem_len,
 436                                          (dma_addr_t *)&info->fix.smem_start,
 437                                          GFP_KERNEL);
 438
 439         if (!info->screen_base) {
 440                 return -ENOMEM;
 441         }
 442
 443         memset(info->screen_base, 0, info->fix.smem_len);
 444
 445         return 0;
 446 }
 447
 448 static const struct fb_videomode *atmel_lcdfb_choose_mode(struct fb_var_screeninfo *var,
 449                                                      struct fb_info *info)
 450 {
 451         struct fb_videomode varfbmode;
 452         const struct fb_videomode *fbmode = NULL;
 453
 454         fb_var_to_videomode(&varfbmode, var);
 455         fbmode = fb_find_nearest_mode(&varfbmode, &info->modelist);
 456         if (fbmode)
 457                 fb_videomode_to_var(var, fbmode);
 458         return fbmode;
 459 }
 460
 461
 462 /**
 463  *      atmel_lcdfb_check_var - Validates a var passed in.
 464  *      @var: frame buffer variable screen structure
 465  *      @info: frame buffer structure that represents a single frame buffer
 466  *
 467  *      Checks to see if the hardware supports the state requested by
 468  *      var passed in. This function does not alter the hardware
 469  *      state!!!  This means the data stored in struct fb_info and
 470  *      struct atmel_lcdfb_info do not change. This includes the var
 471  *      inside of struct fb_info.  Do NOT change these. This function
 472  *      can be called on its own if we intent to only test a mode and
 473  *      not actually set it. The stuff in modedb.c is a example of
 474  *      this. If the var passed in is slightly off by what the
 475  *      hardware can support then we alter the var PASSED in to what
 476  *      we can do. If the hardware doesn't support mode change a
 477  *      -EINVAL will be returned by the upper layers. You don't need
 478  *      to implement this function then. If you hardware doesn't
 479  *      support changing the resolution then this function is not
 480  *      needed. In this case the driver would just provide a var that
 481  *      represents the static state the screen is in.
 482  *
 483  *      Returns negative errno on error, or zero on success.
 484  */
 485 static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
 486                              struct fb_info *info)
 487 {
 488         struct device *dev = info->device;
 489         struct atmel_lcdfb_info *sinfo = info->par;
 490         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 491         unsigned long clk_value_khz;
 492
 493         clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;
 494
 495         dev_dbg(dev, "%s:\n", __func__);
 496
 497         if (!(var->pixclock && var->bits_per_pixel)) {
 498                 /* choose a suitable mode if possible */
 499                 if (!atmel_lcdfb_choose_mode(var, info)) {
 500                         dev_err(dev, "needed value not specified\n");
 501                         return -EINVAL;
 502                 }
 503         }
 504
 505         dev_dbg(dev, "  resolution: %ux%u\n", var->xres, var->yres);
 506         dev_dbg(dev, "  pixclk:     %lu KHz\n", PICOS2KHZ(var->pixclock));
 507         dev_dbg(dev, "  bpp:        %u\n", var->bits_per_pixel);
 508         dev_dbg(dev, "  clk:        %lu KHz\n", clk_value_khz);
 509
 510         if (PICOS2KHZ(var->pixclock) > clk_value_khz) {
 511                 dev_err(dev, "%lu KHz pixel clock is too fast\n", PICOS2KHZ(var->pixclock));
 512                 return -EINVAL;
 513         }
 514
 515         /* Do not allow to have real resoulution larger than virtual */
 516         if (var->xres > var->xres_virtual)
 517                 var->xres_virtual = var->xres;
 518
 519         if (var->yres > var->yres_virtual)
 520                 var->yres_virtual = var->yres;
 521
 522         /* Force same alignment for each line */
 523         var->xres = (var->xres + 3) & ~3UL;
 524         var->xres_virtual = (var->xres_virtual + 3) & ~3UL;
 525
 526         var->red.msb_right = var->green.msb_right = var->blue.msb_right = 0;
 527         var->transp.msb_right = 0;
 528         var->transp.offset = var->transp.length = 0;
 529         var->xoffset = var->yoffset = 0;
 530
 531         if (info->fix.smem_len) {
 532                 unsigned int smem_len = (var->xres_virtual * var->yres_virtual
 533                                          * ((var->bits_per_pixel + 7) / 8));
 534                 if (smem_len > info->fix.smem_len) {
 535                         dev_err(dev, "Frame buffer is too small (%u) for screen size (need at least %u)\n",
 536                                 info->fix.smem_len, smem_len);
 537                         return -EINVAL;
 538                 }
 539         }
 540
 541         /* Saturate vertical and horizontal timings at maximum values */
 542         var->vsync_len = min_t(u32, var->vsync_len,
 543                         (ATMEL_LCDC_VPW >> ATMEL_LCDC_VPW_OFFSET) + 1);
 544         var->upper_margin = min_t(u32, var->upper_margin,
 545                         ATMEL_LCDC_VBP >> ATMEL_LCDC_VBP_OFFSET);
 546         var->lower_margin = min_t(u32, var->lower_margin,
 547                         ATMEL_LCDC_VFP);
 548         var->right_margin = min_t(u32, var->right_margin,
 549                         (ATMEL_LCDC_HFP >> ATMEL_LCDC_HFP_OFFSET) + 1);
 550         var->hsync_len = min_t(u32, var->hsync_len,
 551                         (ATMEL_LCDC_HPW >> ATMEL_LCDC_HPW_OFFSET) + 1);
 552         var->left_margin = min_t(u32, var->left_margin,
 553                         ATMEL_LCDC_HBP + 1);
 554
 555         /* Some parameters can't be zero */
 556         var->vsync_len = max_t(u32, var->vsync_len, 1);
 557         var->right_margin = max_t(u32, var->right_margin, 1);
 558         var->hsync_len = max_t(u32, var->hsync_len, 1);
 559         var->left_margin = max_t(u32, var->left_margin, 1);
 560
 561         switch (var->bits_per_pixel) {
 562         case 1:
 563         case 2:
 564         case 4:
 565         case 8:
 566                 var->red.offset = var->green.offset = var->blue.offset = 0;
 567                 var->red.length = var->green.length = var->blue.length
 568                         = var->bits_per_pixel;
 569                 break;
 570         case 16:
 571                 /* Older SOCs use IBGR:555 rather than BGR:565. */
 572                 if (sinfo->config->have_intensity_bit)
 573                         var->green.length = 5;
 574                 else
 575                         var->green.length = 6;
 576
 577                 if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
 578                         /* RGB:5X5 mode */
 579                         var->red.offset = var->green.length + 5;
 580                         var->blue.offset = 0;
 581                 } else {
 582                         /* BGR:5X5 mode */
 583                         var->red.offset = 0;
 584                         var->blue.offset = var->green.length + 5;
 585                 }
 586                 var->green.offset = 5;
 587                 var->red.length = var->blue.length = 5;
 588                 break;
 589         case 32:
 590                 var->transp.offset = 24;
 591                 var->transp.length = 8;
 592                 /* fall through */
 593         case 24:
 594                 if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
 595                         /* RGB:888 mode */
 596                         var->red.offset = 16;
 597                         var->blue.offset = 0;
 598                 } else {
 599                         /* BGR:888 mode */
 600                         var->red.offset = 0;
 601                         var->blue.offset = 16;
 602                 }
 603                 var->green.offset = 8;
 604                 var->red.length = var->green.length = var->blue.length = 8;
 605                 break;
 606         default:
 607                 dev_err(dev, "color depth %d not supported\n",
 608                                         var->bits_per_pixel);
 609                 return -EINVAL;
 610         }
 611
 612         return 0;
 613 }
 614
 615 /*
 616  * LCD reset sequence
 617  */
 618 static void atmel_lcdfb_reset(struct atmel_lcdfb_info *sinfo)
 619 {
 620         might_sleep();
 621
 622         atmel_lcdfb_stop(sinfo);
 623         atmel_lcdfb_start(sinfo);
 624 }
 625
 626 /**
 627  *      atmel_lcdfb_set_par - Alters the hardware state.
 628  *      @info: frame buffer structure that represents a single frame buffer
 629  *
 630  *      Using the fb_var_screeninfo in fb_info we set the resolution
 631  *      of the this particular framebuffer. This function alters the
 632  *      par AND the fb_fix_screeninfo stored in fb_info. It doesn't
 633  *      not alter var in fb_info since we are using that data. This
 634  *      means we depend on the data in var inside fb_info to be
 635  *      supported by the hardware.  atmel_lcdfb_check_var is always called
 636  *      before atmel_lcdfb_set_par to ensure this.  Again if you can't
 637  *      change the resolution you don't need this function.
 638  *
 639  */
 640 static int atmel_lcdfb_set_par(struct fb_info *info)
 641 {
 642         struct atmel_lcdfb_info *sinfo = info->par;
 643         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 644         unsigned long hozval_linesz;
 645         unsigned long value;
 646         unsigned long clk_value_khz;
 647         unsigned long bits_per_line;
 648         unsigned long pix_factor = 2;
 649
 650         might_sleep();
 651
 652         dev_dbg(info->device, "%s:\n", __func__);
 653         dev_dbg(info->device, "  * resolution: %ux%u (%ux%u virtual)\n",
 654                  info->var.xres, info->var.yres,
 655                  info->var.xres_virtual, info->var.yres_virtual);
 656
 657         atmel_lcdfb_stop_nowait(sinfo);
 658
 659         if (info->var.bits_per_pixel == 1)
 660                 info->fix.visual = FB_VISUAL_MONO01;
 661         else if (info->var.bits_per_pixel <= 8)
 662                 info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
 663         else
 664                 info->fix.visual = FB_VISUAL_TRUECOLOR;
 665
 666         bits_per_line = info->var.xres_virtual * info->var.bits_per_pixel;
 667         info->fix.line_length = DIV_ROUND_UP(bits_per_line, 8);
 668
 669         /* Re-initialize the DMA engine... */
 670         dev_dbg(info->device, "  * update DMA engine\n");
 671         atmel_lcdfb_update_dma(info, &info->var);
 672
 673         /* ...set frame size and burst length = 8 words (?) */
 674         value = (info->var.yres * info->var.xres * info->var.bits_per_pixel) / 32;
 675         value |= ((ATMEL_LCDC_DMA_BURST_LEN - 1) << ATMEL_LCDC_BLENGTH_OFFSET);
 676         lcdc_writel(sinfo, ATMEL_LCDC_DMAFRMCFG, value);
 677
 678         /* Now, the LCDC core... */
 679
 680         /* Set pixel clock */
 681         if (sinfo->config->have_alt_pixclock)
 682                 pix_factor = 1;
 683
 684         clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;
 685
 686         value = DIV_ROUND_UP(clk_value_khz, PICOS2KHZ(info->var.pixclock));
 687
 688         if (value < pix_factor) {
 689                 dev_notice(info->device, "Bypassing pixel clock divider\n");
 690                 lcdc_writel(sinfo, ATMEL_LCDC_LCDCON1, ATMEL_LCDC_BYPASS);
 691         } else {
 692                 value = (value / pix_factor) - 1;
 693                 dev_dbg(info->device, "  * programming CLKVAL = 0x%08lx\n",
 694                                 value);
 695                 lcdc_writel(sinfo, ATMEL_LCDC_LCDCON1,
 696                                 value << ATMEL_LCDC_CLKVAL_OFFSET);
 697                 info->var.pixclock =
 698                         KHZ2PICOS(clk_value_khz / (pix_factor * (value + 1)));
 699                 dev_dbg(info->device, "  updated pixclk:     %lu KHz\n",
 700                                         PICOS2KHZ(info->var.pixclock));
 701         }
 702
 703
 704         /* Initialize control register 2 */
 705         value = pdata->default_lcdcon2;
 706
 707         if (!(info->var.sync & FB_SYNC_HOR_HIGH_ACT))
 708                 value |= ATMEL_LCDC_INVLINE_INVERTED;
 709         if (!(info->var.sync & FB_SYNC_VERT_HIGH_ACT))
 710                 value |= ATMEL_LCDC_INVFRAME_INVERTED;
 711
 712         switch (info->var.bits_per_pixel) {
 713                 case 1: value |= ATMEL_LCDC_PIXELSIZE_1; break;
 714                 case 2: value |= ATMEL_LCDC_PIXELSIZE_2; break;
 715                 case 4: value |= ATMEL_LCDC_PIXELSIZE_4; break;
 716                 case 8: value |= ATMEL_LCDC_PIXELSIZE_8; break;
 717                 case 15: /* fall through */
 718                 case 16: value |= ATMEL_LCDC_PIXELSIZE_16; break;
 719                 case 24: value |= ATMEL_LCDC_PIXELSIZE_24; break;
 720                 case 32: value |= ATMEL_LCDC_PIXELSIZE_32; break;
 721                 default: BUG(); break;
 722         }
 723         dev_dbg(info->device, "  * LCDCON2 = %08lx\n", value);
 724         lcdc_writel(sinfo, ATMEL_LCDC_LCDCON2, value);
 725
 726         /* Vertical timing */
 727         value = (info->var.vsync_len - 1) << ATMEL_LCDC_VPW_OFFSET;
 728         value |= info->var.upper_margin << ATMEL_LCDC_VBP_OFFSET;
 729         value |= info->var.lower_margin;
 730         dev_dbg(info->device, "  * LCDTIM1 = %08lx\n", value);
 731         lcdc_writel(sinfo, ATMEL_LCDC_TIM1, value);
 732
 733         /* Horizontal timing */
 734         value = (info->var.right_margin - 1) << ATMEL_LCDC_HFP_OFFSET;
 735         value |= (info->var.hsync_len - 1) << ATMEL_LCDC_HPW_OFFSET;
 736         value |= (info->var.left_margin - 1);
 737         dev_dbg(info->device, "  * LCDTIM2 = %08lx\n", value);
 738         lcdc_writel(sinfo, ATMEL_LCDC_TIM2, value);
 739
 740         /* Horizontal value (aka line size) */
 741         hozval_linesz = compute_hozval(sinfo, info->var.xres);
 742
 743         /* Display size */
 744         value = (hozval_linesz - 1) << ATMEL_LCDC_HOZVAL_OFFSET;
 745         value |= info->var.yres - 1;
 746         dev_dbg(info->device, "  * LCDFRMCFG = %08lx\n", value);
 747         lcdc_writel(sinfo, ATMEL_LCDC_LCDFRMCFG, value);
 748
 749         /* FIFO Threshold: Use formula from data sheet */
 750         value = ATMEL_LCDC_FIFO_SIZE - (2 * ATMEL_LCDC_DMA_BURST_LEN + 3);
 751         lcdc_writel(sinfo, ATMEL_LCDC_FIFO, value);
 752
 753         /* Toggle LCD_MODE every frame */
 754         lcdc_writel(sinfo, ATMEL_LCDC_MVAL, 0);
 755
 756         /* Disable all interrupts */
 757         lcdc_writel(sinfo, ATMEL_LCDC_IDR, ~0UL);
 758         /* Enable FIFO & DMA errors */
 759         lcdc_writel(sinfo, ATMEL_LCDC_IER, ATMEL_LCDC_UFLWI | ATMEL_LCDC_OWRI | ATMEL_LCDC_MERI);
 760
 761         /* ...wait for DMA engine to become idle... */
 762         while (lcdc_readl(sinfo, ATMEL_LCDC_DMACON) & ATMEL_LCDC_DMABUSY)
 763                 msleep(10);
 764
 765         atmel_lcdfb_start(sinfo);
 766
 767         dev_dbg(info->device, "  * DONE\n");
 768
 769         return 0;
 770 }
 771
 772 static inline unsigned int chan_to_field(unsigned int chan, const struct fb_bitfield *bf)
 773 {
 774         chan &= 0xffff;
 775         chan >>= 16 - bf->length;
 776         return chan << bf->offset;
 777 }
 778
 779 /**
 780  *      atmel_lcdfb_setcolreg - Optional function. Sets a color register.
 781  *      @regno: Which register in the CLUT we are programming
 782  *      @red: The red value which can be up to 16 bits wide
 783  *      @green: The green value which can be up to 16 bits wide
 784  *      @blue:  The blue value which can be up to 16 bits wide.
 785  *      @transp: If supported the alpha value which can be up to 16 bits wide.
 786  *      @info: frame buffer info structure
 787  *
 788  *      Set a single color register. The values supplied have a 16 bit
 789  *      magnitude which needs to be scaled in this function for the hardware.
 790  *      Things to take into consideration are how many color registers, if
 791  *      any, are supported with the current color visual. With truecolor mode
 792  *      no color palettes are supported. Here a pseudo palette is created
 793  *      which we store the value in pseudo_palette in struct fb_info. For
 794  *      pseudocolor mode we have a limited color palette. To deal with this
 795  *      we can program what color is displayed for a particular pixel value.
 796  *      DirectColor is similar in that we can program each color field. If
 797  *      we have a static colormap we don't need to implement this function.
 798  *
 799  *      Returns negative errno on error, or zero on success. In an
 800  *      ideal world, this would have been the case, but as it turns
 801  *      out, the other drivers return 1 on failure, so that's what
 802  *      we're going to do.
 803  */
 804 static int atmel_lcdfb_setcolreg(unsigned int regno, unsigned int red,
 805                              unsigned int green, unsigned int blue,
 806                              unsigned int transp, struct fb_info *info)
 807 {
 808         struct atmel_lcdfb_info *sinfo = info->par;
 809         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
 810         unsigned int val;
 811         u32 *pal;
 812         int ret = 1;
 813
 814         if (info->var.grayscale)
 815                 red = green = blue = (19595 * red + 38470 * green
 816                                       + 7471 * blue) >> 16;
 817
 818         switch (info->fix.visual) {
 819         case FB_VISUAL_TRUECOLOR:
 820                 if (regno < 16) {
 821                         pal = info->pseudo_palette;
 822
 823                         val  = chan_to_field(red, &info->var.red);
 824                         val |= chan_to_field(green, &info->var.green);
 825                         val |= chan_to_field(blue, &info->var.blue);
 826
 827                         pal[regno] = val;
 828                         ret = 0;
 829                 }
 830                 break;
 831
 832         case FB_VISUAL_PSEUDOCOLOR:
 833                 if (regno < 256) {
 834                         if (sinfo->config->have_intensity_bit) {
 835                                 /* old style I+BGR:555 */
 836                                 val  = ((red   >> 11) & 0x001f);
 837                                 val |= ((green >>  6) & 0x03e0);
 838                                 val |= ((blue  >>  1) & 0x7c00);
 839
 840                                 /*
 841                                  * TODO: intensity bit. Maybe something like
 842                                  *   ~(red[10] ^ green[10] ^ blue[10]) & 1
 843                                  */
 844                         } else {
 845                                 /* new style BGR:565 / RGB:565 */
 846                                 if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
 847                                         val  = ((blue >> 11) & 0x001f);
 848                                         val |= ((red  >>  0) & 0xf800);
 849                                 } else {
 850                                         val  = ((red  >> 11) & 0x001f);
 851                                         val |= ((blue >>  0) & 0xf800);
 852                                 }
 853
 854                                 val |= ((green >>  5) & 0x07e0);
 855                         }
 856
 857                         lcdc_writel(sinfo, ATMEL_LCDC_LUT(regno), val);
 858                         ret = 0;
 859                 }
 860                 break;
 861
 862         case FB_VISUAL_MONO01:
 863                 if (regno < 2) {
 864                         val = (regno == 0) ? 0x00 : 0x1F;
 865                         lcdc_writel(sinfo, ATMEL_LCDC_LUT(regno), val);
 866                         ret = 0;
 867                 }
 868                 break;
 869
 870         }
 871
 872         return ret;
 873 }
 874
 875 static int atmel_lcdfb_pan_display(struct fb_var_screeninfo *var,
 876                                struct fb_info *info)
 877 {
 878         dev_dbg(info->device, "%s\n", __func__);
 879
 880         atmel_lcdfb_update_dma(info, var);
 881
 882         return 0;
 883 }
 884
 885 static int atmel_lcdfb_blank(int blank_mode, struct fb_info *info)
 886 {
 887         struct atmel_lcdfb_info *sinfo = info->par;
 888
 889         switch (blank_mode) {
 890         case FB_BLANK_UNBLANK:
 891         case FB_BLANK_NORMAL:
 892                 atmel_lcdfb_start(sinfo);
 893                 break;
 894         case FB_BLANK_VSYNC_SUSPEND:
 895         case FB_BLANK_HSYNC_SUSPEND:
 896                 break;
 897         case FB_BLANK_POWERDOWN:
 898                 atmel_lcdfb_stop(sinfo);
 899                 break;
 900         default:
 901                 return -EINVAL;
 902         }
 903
 904         /* let fbcon do a soft blank for us */
 905         return ((blank_mode == FB_BLANK_NORMAL) ? 1 : 0);
 906 }
 907
 908 static struct fb_ops atmel_lcdfb_ops = {
 909         .owner          = THIS_MODULE,
 910         .fb_check_var   = atmel_lcdfb_check_var,
 911         .fb_set_par     = atmel_lcdfb_set_par,
 912         .fb_setcolreg   = atmel_lcdfb_setcolreg,
 913         .fb_blank       = atmel_lcdfb_blank,
 914         .fb_pan_display = atmel_lcdfb_pan_display,
 915         .fb_fillrect    = cfb_fillrect,
 916         .fb_copyarea    = cfb_copyarea,
 917         .fb_imageblit   = cfb_imageblit,
 918 };
 919
 920 static irqreturn_t atmel_lcdfb_interrupt(int irq, void *dev_id)
 921 {
 922         struct fb_info *info = dev_id;
 923         struct atmel_lcdfb_info *sinfo = info->par;
 924         u32 status;
 925
 926         status = lcdc_readl(sinfo, ATMEL_LCDC_ISR);
 927         if (status & ATMEL_LCDC_UFLWI) {
 928                 dev_warn(info->device, "FIFO underflow %#x\n", status);
 929                 /* reset DMA and FIFO to avoid screen shifting */
 930                 schedule_work(&sinfo->task);
 931         }
 932         lcdc_writel(sinfo, ATMEL_LCDC_ICR, status);
 933         return IRQ_HANDLED;
 934 }
 935
 936 /*
 937  * LCD controller task (to reset the LCD)
 938  */
 939 static void atmel_lcdfb_task(struct work_struct *work)
 940 {
 941         struct atmel_lcdfb_info *sinfo =
 942                 container_of(work, struct atmel_lcdfb_info, task);
 943
 944         atmel_lcdfb_reset(sinfo);
 945 }
 946
 947 static int __init atmel_lcdfb_init_fbinfo(struct atmel_lcdfb_info *sinfo)
 948 {
 949         struct fb_info *info = sinfo->info;
 950         int ret = 0;
 951
 952         info->var.activate |= FB_ACTIVATE_FORCE | FB_ACTIVATE_NOW;
 953
 954         dev_info(info->device,
 955                "%luKiB frame buffer at %08lx (mapped at %p)\n",
 956                (unsigned long)info->fix.smem_len / 1024,
 957                (unsigned long)info->fix.smem_start,
 958                info->screen_base);
 959
 960         /* Allocate colormap */
 961         ret = fb_alloc_cmap(&info->cmap, 256, 0);
 962         if (ret < 0)
 963                 dev_err(info->device, "Alloc color map failed\n");
 964
 965         return ret;
 966 }
 967
 968 static void atmel_lcdfb_start_clock(struct atmel_lcdfb_info *sinfo)
 969 {
 970         clk_prepare_enable(sinfo->bus_clk);
 971         clk_prepare_enable(sinfo->lcdc_clk);
 972 }
 973
 974 static void atmel_lcdfb_stop_clock(struct atmel_lcdfb_info *sinfo)
 975 {
 976         clk_disable_unprepare(sinfo->bus_clk);
 977         clk_disable_unprepare(sinfo->lcdc_clk);
 978 }
 979
 980 #ifdef CONFIG_OF
 981 static const struct of_device_id atmel_lcdfb_dt_ids[] = {
 982         { .compatible = "atmel,at91sam9261-lcdc" , .data = &at91sam9261_config, },
 983         { .compatible = "atmel,at91sam9263-lcdc" , .data = &at91sam9263_config, },
 984         { .compatible = "atmel,at91sam9g10-lcdc" , .data = &at91sam9g10_config, },
 985         { .compatible = "atmel,at91sam9g45-lcdc" , .data = &at91sam9g45_config, },
 986         { .compatible = "atmel,at91sam9g45es-lcdc" , .data = &at91sam9g45es_config, },
 987         { .compatible = "atmel,at91sam9rl-lcdc" , .data = &at91sam9rl_config, },
 988         { .compatible = "atmel,at32ap-lcdc" , .data = &at32ap_config, },
 989         { /* sentinel */ }
 990 };
 991
 992 MODULE_DEVICE_TABLE(of, atmel_lcdfb_dt_ids);
 993
 994 static const char *atmel_lcdfb_wiring_modes[] = {
 995         [ATMEL_LCDC_WIRING_BGR] = "BRG",
 996         [ATMEL_LCDC_WIRING_RGB] = "RGB",
 997 };
 998
 999 static int atmel_lcdfb_get_of_wiring_modes(struct device_node *np)
1000 {
1001         const char *mode;
1002         int err, i;
1003
1004         err = of_property_read_string(np, "atmel,lcd-wiring-mode", &mode);
1005         if (err < 0)
1006                 return ATMEL_LCDC_WIRING_BGR;
1007
1008         for (i = 0; i < ARRAY_SIZE(atmel_lcdfb_wiring_modes); i++)
1009                 if (!strcasecmp(mode, atmel_lcdfb_wiring_modes[i]))
1010                         return i;
1011
1012         return -ENODEV;
1013 }
1014
1015 static void atmel_lcdfb_power_control_gpio(struct atmel_lcdfb_pdata *pdata, int on)
1016 {
1017         struct atmel_lcdfb_power_ctrl_gpio *og;
1018
1019         list_for_each_entry(og, &pdata->pwr_gpios, list)
1020                 gpiod_set_value(og->gpiod, on);
1021 }
1022
1023 static int atmel_lcdfb_of_init(struct atmel_lcdfb_info *sinfo)
1024 {
1025         struct fb_info *info = sinfo->info;
1026         struct atmel_lcdfb_pdata *pdata = &sinfo->pdata;
1027         struct fb_var_screeninfo *var = &info->var;
1028         struct device *dev = &sinfo->pdev->dev;
1029         struct device_node *np =dev->of_node;
1030         struct device_node *display_np;
1031         struct device_node *timings_np;
1032         struct display_timings *timings;
1033         struct atmel_lcdfb_power_ctrl_gpio *og;
1034         bool is_gpio_power = false;
1035         struct gpio_desc *gpiod;
1036         int ret = -ENOENT;
1037         int i;
1038
1039         sinfo->config = (struct atmel_lcdfb_config*)
1040                 of_match_device(atmel_lcdfb_dt_ids, dev)->data;
1041
1042         display_np = of_parse_phandle(np, "display", 0);
1043         if (!display_np) {
1044                 dev_err(dev, "failed to find display phandle\n");
1045                 return -ENOENT;
1046         }
1047
1048         ret = of_property_read_u32(display_np, "bits-per-pixel", &var->bits_per_pixel);
1049         if (ret < 0) {
1050                 dev_err(dev, "failed to get property bits-per-pixel\n");
1051                 goto put_display_node;
1052         }
1053
1054         ret = of_property_read_u32(display_np, "atmel,guard-time", &pdata->guard_time);
1055         if (ret < 0) {
1056                 dev_err(dev, "failed to get property atmel,guard-time\n");
1057                 goto put_display_node;
1058         }
1059
1060         ret = of_property_read_u32(display_np, "atmel,lcdcon2", &pdata->default_lcdcon2);
1061         if (ret < 0) {
1062                 dev_err(dev, "failed to get property atmel,lcdcon2\n");
1063                 goto put_display_node;
1064         }
1065
1066         ret = of_property_read_u32(display_np, "atmel,dmacon", &pdata->default_dmacon);
1067         if (ret < 0) {
1068                 dev_err(dev, "failed to get property bits-per-pixel\n");
1069                 goto put_display_node;
1070         }
1071
1072         INIT_LIST_HEAD(&pdata->pwr_gpios);
1073         ret = -ENOMEM;
1074         for (i = 0; i < gpiod_count(dev, "atmel,power-control"); i++) {
1075                 gpiod = devm_gpiod_get_index(dev, "atmel,power-control",
1076                                              i, GPIOD_ASIS);
1077                 if (IS_ERR(gpiod))
1078                         continue;
1079
1080                 og = devm_kzalloc(dev, sizeof(*og), GFP_KERNEL);
1081                 if (!og)
1082                         goto put_display_node;
1083
1084                 og->gpiod = gpiod;
1085                 is_gpio_power = true;
1086
1087                 ret = gpiod_direction_output(gpiod, gpiod_is_active_low(gpiod));
1088                 if (ret) {
1089                         dev_err(dev, "set direction output gpio atmel,power-control[%d] failed\n", i);
1090                         goto put_display_node;
1091                 }
1092                 list_add(&og->list, &pdata->pwr_gpios);
1093         }
1094
1095         if (is_gpio_power)
1096                 pdata->atmel_lcdfb_power_control = atmel_lcdfb_power_control_gpio;
1097
1098         ret = atmel_lcdfb_get_of_wiring_modes(display_np);
1099         if (ret < 0) {
1100                 dev_err(dev, "invalid atmel,lcd-wiring-mode\n");
1101                 goto put_display_node;
1102         }
1103         pdata->lcd_wiring_mode = ret;
1104
1105         pdata->lcdcon_is_backlight = of_property_read_bool(display_np, "atmel,lcdcon-backlight");
1106         pdata->lcdcon_pol_negative = of_property_read_bool(display_np, "atmel,lcdcon-backlight-inverted");
1107
1108         timings = of_get_display_timings(display_np);
1109         if (!timings) {
1110                 dev_err(dev, "failed to get display timings\n");
1111                 ret = -EINVAL;
1112                 goto put_display_node;
1113         }
1114
1115         timings_np = of_get_child_by_name(display_np, "display-timings");
1116         if (!timings_np) {
1117                 dev_err(dev, "failed to find display-timings node\n");
1118                 ret = -ENODEV;
1119                 goto put_display_node;
1120         }
1121
1122         for (i = 0; i < of_get_child_count(timings_np); i++) {
1123                 struct videomode vm;
1124                 struct fb_videomode fb_vm;
1125
1126                 ret = videomode_from_timings(timings, &vm, i);
1127                 if (ret < 0)
1128                         goto put_timings_node;
1129                 ret = fb_videomode_from_videomode(&vm, &fb_vm);
1130                 if (ret < 0)
1131                         goto put_timings_node;
1132
1133                 fb_add_videomode(&fb_vm, &info->modelist);
1134         }
1135
1136         /*
1137          * FIXME: Make sure we are not referencing any fields in display_np
1138          * and timings_np and drop our references to them before returning to
1139          * avoid leaking the nodes on probe deferral and driver unbind.
1140          */
1141
1142         return 0;
1143
1144 put_timings_node:
1145         of_node_put(timings_np);
1146 put_display_node:
1147         of_node_put(display_np);
1148         return ret;
1149 }
1150 #else
1151 static int atmel_lcdfb_of_init(struct atmel_lcdfb_info *sinfo)
1152 {
1153         return 0;
1154 }
1155 #endif
1156
1157 static int __init atmel_lcdfb_probe(struct platform_device *pdev)
1158 {
1159         struct device *dev = &pdev->dev;
1160         struct fb_info *info;
1161         struct atmel_lcdfb_info *sinfo;
1162         struct atmel_lcdfb_pdata *pdata = NULL;
1163         struct resource *regs = NULL;
1164         struct resource *map = NULL;
1165         struct fb_modelist *modelist;
1166         int ret;
1167
1168         dev_dbg(dev, "%s BEGIN\n", __func__);
1169
1170         ret = -ENOMEM;
1171         info = framebuffer_alloc(sizeof(struct atmel_lcdfb_info), dev);
1172         if (!info) {
1173                 dev_err(dev, "cannot allocate memory\n");
1174                 goto out;
1175         }
1176
1177         sinfo = info->par;
1178         sinfo->pdev = pdev;
1179         sinfo->info = info;
1180
1181         INIT_LIST_HEAD(&info->modelist);
1182
1183         if (pdev->dev.of_node) {
1184                 ret = atmel_lcdfb_of_init(sinfo);
1185                 if (ret)
1186                         goto free_info;
1187         } else if (dev_get_platdata(dev)) {
1188                 struct fb_monspecs *monspecs;
1189                 int i;
1190
1191                 pdata = dev_get_platdata(dev);
1192                 monspecs = pdata->default_monspecs;
1193                 sinfo->pdata = *pdata;
1194
1195                 for (i = 0; i < monspecs->modedb_len; i++)
1196                         fb_add_videomode(&monspecs->modedb[i], &info->modelist);
1197
1198                 sinfo->config = atmel_lcdfb_get_config(pdev);
1199
1200                 info->var.bits_per_pixel = pdata->default_bpp ? pdata->default_bpp : 16;
1201                 memcpy(&info->monspecs, pdata->default_monspecs, sizeof(info->monspecs));
1202         } else {
1203                 dev_err(dev, "cannot get default configuration\n");
1204                 goto free_info;
1205         }
1206
1207         if (!sinfo->config)
1208                 goto free_info;
1209
1210         sinfo->reg_lcd = devm_regulator_get(&pdev->dev, "lcd");
1211         if (IS_ERR(sinfo->reg_lcd))
1212                 sinfo->reg_lcd = NULL;
1213
1214         info->flags = ATMEL_LCDFB_FBINFO_DEFAULT;
1215         info->pseudo_palette = sinfo->pseudo_palette;
1216         info->fbops = &atmel_lcdfb_ops;
1217
1218         info->fix = atmel_lcdfb_fix;
1219         strcpy(info->fix.id, sinfo->pdev->name);
1220
1221         /* Enable LCDC Clocks */
1222         sinfo->bus_clk = clk_get(dev, "hclk");
1223         if (IS_ERR(sinfo->bus_clk)) {
1224                 ret = PTR_ERR(sinfo->bus_clk);
1225                 goto free_info;
1226         }
1227         sinfo->lcdc_clk = clk_get(dev, "lcdc_clk");
1228         if (IS_ERR(sinfo->lcdc_clk)) {
1229                 ret = PTR_ERR(sinfo->lcdc_clk);
1230                 goto put_bus_clk;
1231         }
1232         atmel_lcdfb_start_clock(sinfo);
1233
1234         modelist = list_first_entry(&info->modelist,
1235                         struct fb_modelist, list);
1236         fb_videomode_to_var(&info->var, &modelist->mode);
1237
1238         atmel_lcdfb_check_var(&info->var, info);
1239
1240         regs = platform_get_resource(pdev, IORESOURCE_MEM, 0);
1241         if (!regs) {
1242                 dev_err(dev, "resources unusable\n");
1243                 ret = -ENXIO;
1244                 goto stop_clk;
1245         }
1246
1247         sinfo->irq_base = platform_get_irq(pdev, 0);
1248         if (sinfo->irq_base < 0) {
1249                 dev_err(dev, "unable to get irq\n");
1250                 ret = sinfo->irq_base;
1251                 goto stop_clk;
1252         }
1253
1254         /* Initialize video memory */
1255         map = platform_get_resource(pdev, IORESOURCE_MEM, 1);
1256         if (map) {
1257                 /* use a pre-allocated memory buffer */
1258                 info->fix.smem_start = map->start;
1259                 info->fix.smem_len = resource_size(map);
1260                 if (!request_mem_region(info->fix.smem_start,
1261                                         info->fix.smem_len, pdev->name)) {
1262                         ret = -EBUSY;
1263                         goto stop_clk;
1264                 }
1265
1266                 info->screen_base = ioremap_wc(info->fix.smem_start,
1267                                                info->fix.smem_len);
1268                 if (!info->screen_base) {
1269                         ret = -ENOMEM;
1270                         goto release_intmem;
1271                 }
1272
1273                 /*
1274                  * Don't clear the framebuffer -- someone may have set
1275                  * up a splash image.
1276                  */
1277         } else {
1278                 /* allocate memory buffer */
1279                 ret = atmel_lcdfb_alloc_video_memory(sinfo);
1280                 if (ret < 0) {
1281                         dev_err(dev, "cannot allocate framebuffer: %d\n", ret);
1282                         goto stop_clk;
1283                 }
1284         }
1285
1286         /* LCDC registers */
1287         info->fix.mmio_start = regs->start;
1288         info->fix.mmio_len = resource_size(regs);
1289
1290         if (!request_mem_region(info->fix.mmio_start,
1291                                 info->fix.mmio_len, pdev->name)) {
1292                 ret = -EBUSY;
1293                 goto free_fb;
1294         }
1295
1296         sinfo->mmio = ioremap(info->fix.mmio_start, info->fix.mmio_len);
1297         if (!sinfo->mmio) {
1298                 dev_err(dev, "cannot map LCDC registers\n");
1299                 ret = -ENOMEM;
1300                 goto release_mem;
1301         }
1302
1303         /* Initialize PWM for contrast or backlight ("off") */
1304         init_contrast(sinfo);
1305
1306         /* interrupt */
1307         ret = request_irq(sinfo->irq_base, atmel_lcdfb_interrupt, 0, pdev->name, info);
1308         if (ret) {
1309                 dev_err(dev, "request_irq failed: %d\n", ret);
1310                 goto unmap_mmio;
1311         }
1312
1313         /* Some operations on the LCDC might sleep and
1314          * require a preemptible task context */
1315         INIT_WORK(&sinfo->task, atmel_lcdfb_task);
1316
1317         ret = atmel_lcdfb_init_fbinfo(sinfo);
1318         if (ret < 0) {
1319                 dev_err(dev, "init fbinfo failed: %d\n", ret);
1320                 goto unregister_irqs;
1321         }
1322
1323         ret = atmel_lcdfb_set_par(info);
1324         if (ret < 0) {
1325                 dev_err(dev, "set par failed: %d\n", ret);
1326                 goto unregister_irqs;
1327         }
1328
1329         dev_set_drvdata(dev, info);
1330
1331         /*
1332          * Tell the world that we're ready to go
1333          */
1334         ret = register_framebuffer(info);
1335         if (ret < 0) {
1336                 dev_err(dev, "failed to register framebuffer device: %d\n", ret);
1337                 goto reset_drvdata;
1338         }
1339
1340         /* Power up the LCDC screen */
1341         atmel_lcdfb_power_control(sinfo, 1);
1342
1343         dev_info(dev, "fb%d: Atmel LCDC at 0x%08lx (mapped at %p), irq %d\n",
1344                        info->node, info->fix.mmio_start, sinfo->mmio, sinfo->irq_base);
1345
1346         return 0;
1347
1348 reset_drvdata:
1349         dev_set_drvdata(dev, NULL);
1350         fb_dealloc_cmap(&info->cmap);
1351 unregister_irqs:
1352         cancel_work_sync(&sinfo->task);
1353         free_irq(sinfo->irq_base, info);
1354 unmap_mmio:
1355         exit_backlight(sinfo);
1356         iounmap(sinfo->mmio);
1357 release_mem:
1358         release_mem_region(info->fix.mmio_start, info->fix.mmio_len);
1359 free_fb:
1360         if (map)
1361                 iounmap(info->screen_base);
1362         else
1363                 atmel_lcdfb_free_video_memory(sinfo);
1364
1365 release_intmem:
1366         if (map)
1367                 release_mem_region(info->fix.smem_start, info->fix.smem_len);
1368 stop_clk:
1369         atmel_lcdfb_stop_clock(sinfo);
1370         clk_put(sinfo->lcdc_clk);
1371 put_bus_clk:
1372         clk_put(sinfo->bus_clk);
1373 free_info:
1374         framebuffer_release(info);
1375 out:
1376         dev_dbg(dev, "%s FAILED\n", __func__);
1377         return ret;
1378 }
1379
1380 static int __exit atmel_lcdfb_remove(struct platform_device *pdev)
1381 {
1382         struct device *dev = &pdev->dev;
1383         struct fb_info *info = dev_get_drvdata(dev);
1384         struct atmel_lcdfb_info *sinfo;
1385         struct atmel_lcdfb_pdata *pdata;
1386
1387         if (!info || !info->par)
1388                 return 0;
1389         sinfo = info->par;
1390         pdata = &sinfo->pdata;
1391
1392         cancel_work_sync(&sinfo->task);
1393         exit_backlight(sinfo);
1394         atmel_lcdfb_power_control(sinfo, 0);
1395         unregister_framebuffer(info);
1396         atmel_lcdfb_stop_clock(sinfo);
1397         clk_put(sinfo->lcdc_clk);
1398         clk_put(sinfo->bus_clk);
1399         fb_dealloc_cmap(&info->cmap);
1400         free_irq(sinfo->irq_base, info);
1401         iounmap(sinfo->mmio);
1402         release_mem_region(info->fix.mmio_start, info->fix.mmio_len);
1403         if (platform_get_resource(pdev, IORESOURCE_MEM, 1)) {
1404                 iounmap(info->screen_base);
1405                 release_mem_region(info->fix.smem_start, info->fix.smem_len);
1406         } else {
1407                 atmel_lcdfb_free_video_memory(sinfo);
1408         }
1409
1410         framebuffer_release(info);
1411
1412         return 0;
1413 }
1414
1415 #ifdef CONFIG_PM
1416
1417 static int atmel_lcdfb_suspend(struct platform_device *pdev, pm_message_t mesg)
1418 {
1419         struct fb_info *info = platform_get_drvdata(pdev);
1420         struct atmel_lcdfb_info *sinfo = info->par;
1421
1422         /*
1423          * We don't want to handle interrupts while the clock is
1424          * stopped. It may take forever.
1425          */
1426         lcdc_writel(sinfo, ATMEL_LCDC_IDR, ~0UL);
1427
1428         sinfo->saved_lcdcon = lcdc_readl(sinfo, ATMEL_LCDC_CONTRAST_CTR);
1429         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, 0);
1430         atmel_lcdfb_power_control(sinfo, 0);
1431         atmel_lcdfb_stop(sinfo);
1432         atmel_lcdfb_stop_clock(sinfo);
1433
1434         return 0;
1435 }
1436
1437 static int atmel_lcdfb_resume(struct platform_device *pdev)
1438 {
1439         struct fb_info *info = platform_get_drvdata(pdev);
1440         struct atmel_lcdfb_info *sinfo = info->par;
1441
1442         atmel_lcdfb_start_clock(sinfo);
1443         atmel_lcdfb_start(sinfo);
1444         atmel_lcdfb_power_control(sinfo, 1);
1445         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, sinfo->saved_lcdcon);
1446
1447         /* Enable FIFO & DMA errors */
1448         lcdc_writel(sinfo, ATMEL_LCDC_IER, ATMEL_LCDC_UFLWI
1449                         | ATMEL_LCDC_OWRI | ATMEL_LCDC_MERI);
1450
1451         return 0;
1452 }
1453
1454 #else
1455 #define atmel_lcdfb_suspend     NULL
1456 #define atmel_lcdfb_resume      NULL
1457 #endif
1458
1459 static struct platform_driver atmel_lcdfb_driver = {
1460         .remove         = __exit_p(atmel_lcdfb_remove),
1461         .suspend        = atmel_lcdfb_suspend,
1462         .resume         = atmel_lcdfb_resume,
1463         .id_table       = atmel_lcdfb_devtypes,
1464         .driver         = {
1465                 .name   = "atmel_lcdfb",
1466                 .of_match_table = of_match_ptr(atmel_lcdfb_dt_ids),
1467         },
1468 };
1469
1470 module_platform_driver_probe(atmel_lcdfb_driver, atmel_lcdfb_probe);
1471
1472 MODULE_DESCRIPTION("AT91/AT32 LCD Controller framebuffer driver");
1473 MODULE_AUTHOR("Nicolas Ferre <nicolas.ferre@atmel.com>");
1474 MODULE_LICENSE("GPL");