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