drivers/video/mmp/fb/mmpfb.c

   1 /*
   2  * linux/drivers/video/mmp/fb/mmpfb.c
   3  * Framebuffer driver for Marvell Display controller.
   4  *
   5  * Copyright (C) 2012 Marvell Technology Group Ltd.
   6  * Authors: Zhou Zhu <zzhu3@marvell.com>
   7  *
   8  * This program is free software; you can redistribute it and/or modify it
   9  * under the terms of the GNU General Public License as published by the
  10  * Free Software Foundation; either version 2 of the License, or (at your
  11  * option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful, but WITHOUT
  14  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  15  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  16  * more details.
  17  *
  18  * You should have received a copy of the GNU General Public License along with
  19  * this program.  If not, see <http://www.gnu.org/licenses/>.
  20  *
  21  */
  22 #include <linux/module.h>
  23 #include <linux/dma-mapping.h>
  24 #include <linux/platform_device.h>
  25 #include "mmpfb.h"
  26
  27 static int var_to_pixfmt(struct fb_var_screeninfo *var)
  28 {
  29         /*
  30          * Pseudocolor mode?
  31          */
  32         if (var->bits_per_pixel == 8)
  33                 return PIXFMT_PSEUDOCOLOR;
  34
  35         /*
  36          * Check for YUV422PLANAR.
  37          */
  38         if (var->bits_per_pixel == 16 && var->red.length == 8 &&
  39                         var->green.length == 4 && var->blue.length == 4) {
  40                 if (var->green.offset >= var->blue.offset)
  41                         return PIXFMT_YUV422P;
  42                 else
  43                         return PIXFMT_YVU422P;
  44         }
  45
  46         /*
  47          * Check for YUV420PLANAR.
  48          */
  49         if (var->bits_per_pixel == 12 && var->red.length == 8 &&
  50                         var->green.length == 2 && var->blue.length == 2) {
  51                 if (var->green.offset >= var->blue.offset)
  52                         return PIXFMT_YUV420P;
  53                 else
  54                         return PIXFMT_YVU420P;
  55         }
  56
  57         /*
  58          * Check for YUV422PACK.
  59          */
  60         if (var->bits_per_pixel == 16 && var->red.length == 16 &&
  61                         var->green.length == 16 && var->blue.length == 16) {
  62                 if (var->red.offset == 0)
  63                         return PIXFMT_YUYV;
  64                 else if (var->green.offset >= var->blue.offset)
  65                         return PIXFMT_UYVY;
  66                 else
  67                         return PIXFMT_VYUY;
  68         }
  69
  70         /*
  71          * Check for 565/1555.
  72          */
  73         if (var->bits_per_pixel == 16 && var->red.length <= 5 &&
  74                         var->green.length <= 6 && var->blue.length <= 5) {
  75                 if (var->transp.length == 0) {
  76                         if (var->red.offset >= var->blue.offset)
  77                                 return PIXFMT_RGB565;
  78                         else
  79                                 return PIXFMT_BGR565;
  80                 }
  81         }
  82
  83         /*
  84          * Check for 888/A888.
  85          */
  86         if (var->bits_per_pixel <= 32 && var->red.length <= 8 &&
  87                         var->green.length <= 8 && var->blue.length <= 8) {
  88                 if (var->bits_per_pixel == 24 && var->transp.length == 0) {
  89                         if (var->red.offset >= var->blue.offset)
  90                                 return PIXFMT_RGB888PACK;
  91                         else
  92                                 return PIXFMT_BGR888PACK;
  93                 }
  94
  95                 if (var->bits_per_pixel == 32 && var->transp.offset == 24) {
  96                         if (var->red.offset >= var->blue.offset)
  97                                 return PIXFMT_RGBA888;
  98                         else
  99                                 return PIXFMT_BGRA888;
 100                 } else {
 101                         if (var->red.offset >= var->blue.offset)
 102                                 return PIXFMT_RGB888UNPACK;
 103                         else
 104                                 return PIXFMT_BGR888UNPACK;
 105                 }
 106
 107                 /* fall through */
 108         }
 109
 110         return -EINVAL;
 111 }
 112
 113 static void pixfmt_to_var(struct fb_var_screeninfo *var, int pix_fmt)
 114 {
 115         switch (pix_fmt) {
 116         case PIXFMT_RGB565:
 117                 var->bits_per_pixel = 16;
 118                 var->red.offset = 11;   var->red.length = 5;
 119                 var->green.offset = 5;   var->green.length = 6;
 120                 var->blue.offset = 0;   var->blue.length = 5;
 121                 var->transp.offset = 0;  var->transp.length = 0;
 122                 break;
 123         case PIXFMT_BGR565:
 124                 var->bits_per_pixel = 16;
 125                 var->red.offset = 0;    var->red.length = 5;
 126                 var->green.offset = 5;   var->green.length = 6;
 127                 var->blue.offset = 11;  var->blue.length = 5;
 128                 var->transp.offset = 0;  var->transp.length = 0;
 129                 break;
 130         case PIXFMT_RGB888UNPACK:
 131                 var->bits_per_pixel = 32;
 132                 var->red.offset = 16;   var->red.length = 8;
 133                 var->green.offset = 8;   var->green.length = 8;
 134                 var->blue.offset = 0;   var->blue.length = 8;
 135                 var->transp.offset = 0;  var->transp.length = 0;
 136                 break;
 137         case PIXFMT_BGR888UNPACK:
 138                 var->bits_per_pixel = 32;
 139                 var->red.offset = 0;    var->red.length = 8;
 140                 var->green.offset = 8;   var->green.length = 8;
 141                 var->blue.offset = 16;  var->blue.length = 8;
 142                 var->transp.offset = 0;  var->transp.length = 0;
 143                 break;
 144         case PIXFMT_RGBA888:
 145                 var->bits_per_pixel = 32;
 146                 var->red.offset = 16;   var->red.length = 8;
 147                 var->green.offset = 8;   var->green.length = 8;
 148                 var->blue.offset = 0;   var->blue.length = 8;
 149                 var->transp.offset = 24; var->transp.length = 8;
 150                 break;
 151         case PIXFMT_BGRA888:
 152                 var->bits_per_pixel = 32;
 153                 var->red.offset = 0;    var->red.length = 8;
 154                 var->green.offset = 8;   var->green.length = 8;
 155                 var->blue.offset = 16;  var->blue.length = 8;
 156                 var->transp.offset = 24; var->transp.length = 8;
 157                 break;
 158         case PIXFMT_RGB888PACK:
 159                 var->bits_per_pixel = 24;
 160                 var->red.offset = 16;   var->red.length = 8;
 161                 var->green.offset = 8;   var->green.length = 8;
 162                 var->blue.offset = 0;   var->blue.length = 8;
 163                 var->transp.offset = 0;  var->transp.length = 0;
 164                 break;
 165         case PIXFMT_BGR888PACK:
 166                 var->bits_per_pixel = 24;
 167                 var->red.offset = 0;    var->red.length = 8;
 168                 var->green.offset = 8;   var->green.length = 8;
 169                 var->blue.offset = 16;  var->blue.length = 8;
 170                 var->transp.offset = 0;  var->transp.length = 0;
 171                 break;
 172         case PIXFMT_YUV420P:
 173                 var->bits_per_pixel = 12;
 174                 var->red.offset = 4;     var->red.length = 8;
 175                 var->green.offset = 2;   var->green.length = 2;
 176                 var->blue.offset = 0;   var->blue.length = 2;
 177                 var->transp.offset = 0;  var->transp.length = 0;
 178                 break;
 179         case PIXFMT_YVU420P:
 180                 var->bits_per_pixel = 12;
 181                 var->red.offset = 4;     var->red.length = 8;
 182                 var->green.offset = 0;   var->green.length = 2;
 183                 var->blue.offset = 2;   var->blue.length = 2;
 184                 var->transp.offset = 0;  var->transp.length = 0;
 185                 break;
 186         case PIXFMT_YUV422P:
 187                 var->bits_per_pixel = 16;
 188                 var->red.offset = 8;     var->red.length = 8;
 189                 var->green.offset = 4;   var->green.length = 4;
 190                 var->blue.offset = 0;   var->blue.length = 4;
 191                 var->transp.offset = 0;  var->transp.length = 0;
 192                 break;
 193         case PIXFMT_YVU422P:
 194                 var->bits_per_pixel = 16;
 195                 var->red.offset = 8;     var->red.length = 8;
 196                 var->green.offset = 0;   var->green.length = 4;
 197                 var->blue.offset = 4;   var->blue.length = 4;
 198                 var->transp.offset = 0;  var->transp.length = 0;
 199                 break;
 200         case PIXFMT_UYVY:
 201                 var->bits_per_pixel = 16;
 202                 var->red.offset = 8;     var->red.length = 16;
 203                 var->green.offset = 4;   var->green.length = 16;
 204                 var->blue.offset = 0;   var->blue.length = 16;
 205                 var->transp.offset = 0;  var->transp.length = 0;
 206                 break;
 207         case PIXFMT_VYUY:
 208                 var->bits_per_pixel = 16;
 209                 var->red.offset = 8;     var->red.length = 16;
 210                 var->green.offset = 0;   var->green.length = 16;
 211                 var->blue.offset = 4;   var->blue.length = 16;
 212                 var->transp.offset = 0;  var->transp.length = 0;
 213                 break;
 214         case PIXFMT_YUYV:
 215                 var->bits_per_pixel = 16;
 216                 var->red.offset = 0;     var->red.length = 16;
 217                 var->green.offset = 4;   var->green.length = 16;
 218                 var->blue.offset = 8;   var->blue.length = 16;
 219                 var->transp.offset = 0;  var->transp.length = 0;
 220                 break;
 221         case PIXFMT_PSEUDOCOLOR:
 222                 var->bits_per_pixel = 8;
 223                 var->red.offset = 0;     var->red.length = 8;
 224                 var->green.offset = 0;   var->green.length = 8;
 225                 var->blue.offset = 0;   var->blue.length = 8;
 226                 var->transp.offset = 0;  var->transp.length = 0;
 227                 break;
 228         }
 229 }
 230
 231 /*
 232  * fb framework has its limitation:
 233  * 1. input color/output color is not seprated
 234  * 2. fb_videomode not include output color
 235  * so for fb usage, we keep a output format which is not changed
 236  *  then it's added for mmpmode
 237  */
 238 static void fbmode_to_mmpmode(struct mmp_mode *mode,
 239                 struct fb_videomode *videomode, int output_fmt)
 240 {
 241         u64 div_result = 1000000000000ll;
 242         mode->name = videomode->name;
 243         mode->refresh = videomode->refresh;
 244         mode->xres = videomode->xres;
 245         mode->yres = videomode->yres;
 246
 247         do_div(div_result, videomode->pixclock);
 248         mode->pixclock_freq = (u32)div_result;
 249
 250         mode->left_margin = videomode->left_margin;
 251         mode->right_margin = videomode->right_margin;
 252         mode->upper_margin = videomode->upper_margin;
 253         mode->lower_margin = videomode->lower_margin;
 254         mode->hsync_len = videomode->hsync_len;
 255         mode->vsync_len = videomode->vsync_len;
 256         mode->hsync_invert = !!(videomode->sync & FB_SYNC_HOR_HIGH_ACT);
 257         mode->vsync_invert = !!(videomode->sync & FB_SYNC_VERT_HIGH_ACT);
 258         /* no defined flag in fb, use vmode>>3*/
 259         mode->invert_pixclock = !!(videomode->vmode & 8);
 260         mode->pix_fmt_out = output_fmt;
 261 }
 262
 263 static void mmpmode_to_fbmode(struct fb_videomode *videomode,
 264                 struct mmp_mode *mode)
 265 {
 266         u64 div_result = 1000000000000ll;
 267
 268         videomode->name = mode->name;
 269         videomode->refresh = mode->refresh;
 270         videomode->xres = mode->xres;
 271         videomode->yres = mode->yres;
 272
 273         do_div(div_result, mode->pixclock_freq);
 274         videomode->pixclock = (u32)div_result;
 275
 276         videomode->left_margin = mode->left_margin;
 277         videomode->right_margin = mode->right_margin;
 278         videomode->upper_margin = mode->upper_margin;
 279         videomode->lower_margin = mode->lower_margin;
 280         videomode->hsync_len = mode->hsync_len;
 281         videomode->vsync_len = mode->vsync_len;
 282         videomode->sync = (mode->hsync_invert ? FB_SYNC_HOR_HIGH_ACT : 0)
 283                 | (mode->vsync_invert ? FB_SYNC_VERT_HIGH_ACT : 0);
 284         videomode->vmode = mode->invert_pixclock ? 8 : 0;
 285 }
 286
 287 static int mmpfb_check_var(struct fb_var_screeninfo *var,
 288                 struct fb_info *info)
 289 {
 290         struct mmpfb_info *fbi = info->par;
 291
 292         if (var->bits_per_pixel == 8)
 293                 return -EINVAL;
 294         /*
 295          * Basic geometry sanity checks.
 296          */
 297         if (var->xoffset + var->xres > var->xres_virtual)
 298                 return -EINVAL;
 299         if (var->yoffset + var->yres > var->yres_virtual)
 300                 return -EINVAL;
 301
 302         /*
 303          * Check size of framebuffer.
 304          */
 305         if (var->xres_virtual * var->yres_virtual *
 306                         (var->bits_per_pixel >> 3) > fbi->fb_size)
 307                 return -EINVAL;
 308
 309         return 0;
 310 }
 311
 312 static unsigned int chan_to_field(unsigned int chan, struct fb_bitfield *bf)
 313 {
 314         return ((chan & 0xffff) >> (16 - bf->length)) << bf->offset;
 315 }
 316
 317 static u32 to_rgb(u16 red, u16 green, u16 blue)
 318 {
 319         red >>= 8;
 320         green >>= 8;
 321         blue >>= 8;
 322
 323         return (red << 16) | (green << 8) | blue;
 324 }
 325
 326 static int mmpfb_setcolreg(unsigned int regno, unsigned int red,
 327                 unsigned int green, unsigned int blue,
 328                 unsigned int trans, struct fb_info *info)
 329 {
 330         struct mmpfb_info *fbi = info->par;
 331         u32 val;
 332
 333         if (info->fix.visual == FB_VISUAL_TRUECOLOR && regno < 16) {
 334                 val =  chan_to_field(red,   &info->var.red);
 335                 val |= chan_to_field(green, &info->var.green);
 336                 val |= chan_to_field(blue , &info->var.blue);
 337                 fbi->pseudo_palette[regno] = val;
 338         }
 339
 340         if (info->fix.visual == FB_VISUAL_PSEUDOCOLOR && regno < 256) {
 341                 val = to_rgb(red, green, blue);
 342                 /* TODO */
 343         }
 344
 345         return 0;
 346 }
 347
 348 static int mmpfb_pan_display(struct fb_var_screeninfo *var,
 349                 struct fb_info *info)
 350 {
 351         struct mmpfb_info *fbi = info->par;
 352         struct mmp_addr addr;
 353
 354         memset(&addr, 0, sizeof(addr));
 355         addr.phys[0] = (var->yoffset * var->xres_virtual + var->xoffset)
 356                 * var->bits_per_pixel / 8 + fbi->fb_start_dma;
 357         mmp_overlay_set_addr(fbi->overlay, &addr);
 358
 359         return 0;
 360 }
 361
 362 static int var_update(struct fb_info *info)
 363 {
 364         struct mmpfb_info *fbi = info->par;
 365         struct fb_var_screeninfo *var = &info->var;
 366         struct fb_videomode *m;
 367         int pix_fmt;
 368
 369         /* set pix_fmt */
 370         pix_fmt = var_to_pixfmt(var);
 371         if (pix_fmt < 0)
 372                 return -EINVAL;
 373         pixfmt_to_var(var, pix_fmt);
 374         fbi->pix_fmt = pix_fmt;
 375
 376         /* set var according to best video mode*/
 377         m = (struct fb_videomode *)fb_match_mode(var, &info->modelist);
 378         if (!m) {
 379                 dev_err(fbi->dev, "set par: no match mode, use best mode\n");
 380                 m = (struct fb_videomode *)fb_find_best_mode(var,
 381                                 &info->modelist);
 382                 fb_videomode_to_var(var, m);
 383         }
 384         memcpy(&fbi->mode, m, sizeof(struct fb_videomode));
 385
 386         /* fix to 2* yres */
 387         var->yres_virtual = var->yres * 2;
 388         info->fix.visual = (pix_fmt == PIXFMT_PSEUDOCOLOR) ?
 389                 FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
 390         info->fix.line_length = var->xres_virtual * var->bits_per_pixel / 8;
 391         info->fix.ypanstep = var->yres;
 392         return 0;
 393 }
 394
 395 static int mmpfb_set_par(struct fb_info *info)
 396 {
 397         struct mmpfb_info *fbi = info->par;
 398         struct fb_var_screeninfo *var = &info->var;
 399         struct mmp_addr addr;
 400         struct mmp_win win;
 401         struct mmp_mode mode;
 402         int ret;
 403
 404         ret = var_update(info);
 405         if (ret != 0)
 406                 return ret;
 407
 408         /* set window/path according to new videomode */
 409         fbmode_to_mmpmode(&mode, &fbi->mode, fbi->output_fmt);
 410         mmp_path_set_mode(fbi->path, &mode);
 411
 412         memset(&win, 0, sizeof(win));
 413         win.xsrc = win.xdst = fbi->mode.xres;
 414         win.ysrc = win.ydst = fbi->mode.yres;
 415         win.pix_fmt = fbi->pix_fmt;
 416         mmp_overlay_set_win(fbi->overlay, &win);
 417
 418         /* set address always */
 419         memset(&addr, 0, sizeof(addr));
 420         addr.phys[0] = (var->yoffset * var->xres_virtual + var->xoffset)
 421                 * var->bits_per_pixel / 8 + fbi->fb_start_dma;
 422         mmp_overlay_set_addr(fbi->overlay, &addr);
 423
 424         return 0;
 425 }
 426
 427 static void mmpfb_power(struct mmpfb_info *fbi, int power)
 428 {
 429         struct mmp_addr addr;
 430         struct mmp_win win;
 431         struct fb_var_screeninfo *var = &fbi->fb_info->var;
 432
 433         /* for power on, always set address/window again */
 434         if (power) {
 435                 memset(&win, 0, sizeof(win));
 436                 win.xsrc = win.xdst = fbi->mode.xres;
 437                 win.ysrc = win.ydst = fbi->mode.yres;
 438                 win.pix_fmt = fbi->pix_fmt;
 439                 mmp_overlay_set_win(fbi->overlay, &win);
 440
 441                 /* set address always */
 442                 memset(&addr, 0, sizeof(addr));
 443                 addr.phys[0] = fbi->fb_start_dma +
 444                         (var->yoffset * var->xres_virtual + var->xoffset)
 445                         * var->bits_per_pixel / 8;
 446                 mmp_overlay_set_addr(fbi->overlay, &addr);
 447         }
 448         mmp_overlay_set_onoff(fbi->overlay, power);
 449 }
 450
 451 static int mmpfb_blank(int blank, struct fb_info *info)
 452 {
 453         struct mmpfb_info *fbi = info->par;
 454
 455         mmpfb_power(fbi, (blank == FB_BLANK_UNBLANK));
 456
 457         return 0;
 458 }
 459
 460 static struct fb_ops mmpfb_ops = {
 461         .owner          = THIS_MODULE,
 462         .fb_blank       = mmpfb_blank,
 463         .fb_check_var   = mmpfb_check_var,
 464         .fb_set_par     = mmpfb_set_par,
 465         .fb_setcolreg   = mmpfb_setcolreg,
 466         .fb_pan_display = mmpfb_pan_display,
 467         .fb_fillrect    = cfb_fillrect,
 468         .fb_copyarea    = cfb_copyarea,
 469         .fb_imageblit   = cfb_imageblit,
 470 };
 471
 472 static int modes_setup(struct mmpfb_info *fbi)
 473 {
 474         struct fb_videomode *videomodes;
 475         struct mmp_mode *mmp_modes;
 476         struct fb_info *info = fbi->fb_info;
 477         int videomode_num, i;
 478
 479         /* get videomodes from path */
 480         videomode_num = mmp_path_get_modelist(fbi->path, &mmp_modes);
 481         if (!videomode_num) {
 482                 dev_warn(fbi->dev, "can't get videomode num\n");
 483                 return 0;
 484         }
 485         /* put videomode list to info structure */
 486         videomodes = kzalloc(sizeof(struct fb_videomode) * videomode_num,
 487                         GFP_KERNEL);
 488         if (!videomodes) {
 489                 dev_err(fbi->dev, "can't malloc video modes\n");
 490                 return -ENOMEM;
 491         }
 492         for (i = 0; i < videomode_num; i++)
 493                 mmpmode_to_fbmode(&videomodes[i], &mmp_modes[i]);
 494         fb_videomode_to_modelist(videomodes, videomode_num, &info->modelist);
 495
 496         /* set videomode[0] as default mode */
 497         memcpy(&fbi->mode, &videomodes[0], sizeof(struct fb_videomode));
 498         fbi->output_fmt = mmp_modes[0].pix_fmt_out;
 499         fb_videomode_to_var(&info->var, &fbi->mode);
 500         mmp_path_set_mode(fbi->path, &mmp_modes[0]);
 501
 502         kfree(videomodes);
 503         return videomode_num;
 504 }
 505
 506 static int fb_info_setup(struct fb_info *info,
 507                         struct mmpfb_info *fbi)
 508 {
 509         int ret = 0;
 510         /* Initialise static fb parameters.*/
 511         info->flags = FBINFO_DEFAULT | FBINFO_PARTIAL_PAN_OK |
 512                 FBINFO_HWACCEL_XPAN | FBINFO_HWACCEL_YPAN;
 513         info->node = -1;
 514         strcpy(info->fix.id, fbi->name);
 515         info->fix.type = FB_TYPE_PACKED_PIXELS;
 516         info->fix.type_aux = 0;
 517         info->fix.xpanstep = 0;
 518         info->fix.ypanstep = info->var.yres;
 519         info->fix.ywrapstep = 0;
 520         info->fix.accel = FB_ACCEL_NONE;
 521         info->fix.smem_start = fbi->fb_start_dma;
 522         info->fix.smem_len = fbi->fb_size;
 523         info->fix.visual = (fbi->pix_fmt == PIXFMT_PSEUDOCOLOR) ?
 524                 FB_VISUAL_PSEUDOCOLOR : FB_VISUAL_TRUECOLOR;
 525         info->fix.line_length = info->var.xres_virtual *
 526                 info->var.bits_per_pixel / 8;
 527         info->fbops = &mmpfb_ops;
 528         info->pseudo_palette = fbi->pseudo_palette;
 529         info->screen_base = fbi->fb_start;
 530         info->screen_size = fbi->fb_size;
 531
 532         /* For FB framework: Allocate color map and Register framebuffer*/
 533         if (fb_alloc_cmap(&info->cmap, 256, 0) < 0)
 534                 ret = -ENOMEM;
 535
 536         return ret;
 537 }
 538
 539 static void fb_info_clear(struct fb_info *info)
 540 {
 541         fb_dealloc_cmap(&info->cmap);
 542 }
 543
 544 static int mmpfb_probe(struct platform_device *pdev)
 545 {
 546         struct mmp_buffer_driver_mach_info *mi;
 547         struct fb_info *info = 0;
 548         struct mmpfb_info *fbi = 0;
 549         int ret, modes_num;
 550
 551         mi = pdev->dev.platform_data;
 552         if (mi == NULL) {
 553                 dev_err(&pdev->dev, "no platform data defined\n");
 554                 return -EINVAL;
 555         }
 556
 557         /* initialize fb */
 558         info = framebuffer_alloc(sizeof(struct mmpfb_info), &pdev->dev);
 559         if (info == NULL)
 560                 return -ENOMEM;
 561         fbi = info->par;
 562         if (!fbi) {
 563                 ret = -EINVAL;
 564                 goto failed;
 565         }
 566
 567         /* init fb */
 568         fbi->fb_info = info;
 569         platform_set_drvdata(pdev, fbi);
 570         fbi->dev = &pdev->dev;
 571         fbi->name = mi->name;
 572         fbi->pix_fmt = mi->default_pixfmt;
 573         pixfmt_to_var(&info->var, fbi->pix_fmt);
 574         mutex_init(&fbi->access_ok);
 575
 576         /* get display path by name */
 577         fbi->path = mmp_get_path(mi->path_name);
 578         if (!fbi->path) {
 579                 dev_err(&pdev->dev, "can't get the path %s\n", mi->path_name);
 580                 ret = -EINVAL;
 581                 goto failed_destroy_mutex;
 582         }
 583
 584         dev_info(fbi->dev, "path %s get\n", fbi->path->name);
 585
 586         /* get overlay */
 587         fbi->overlay = mmp_path_get_overlay(fbi->path, mi->overlay_id);
 588         if (!fbi->overlay) {
 589                 ret = -EINVAL;
 590                 goto failed_destroy_mutex;
 591         }
 592         /* set fetch used */
 593         mmp_overlay_set_fetch(fbi->overlay, mi->dmafetch_id);
 594
 595         modes_num = modes_setup(fbi);
 596         if (modes_num < 0) {
 597                 ret = modes_num;
 598                 goto failed_destroy_mutex;
 599         }
 600
 601         /*
 602          * if get modes success, means not hotplug panels, use caculated buffer
 603          * or use default size
 604          */
 605         if (modes_num > 0) {
 606                 /* fix to 2* yres */
 607                 info->var.yres_virtual = info->var.yres * 2;
 608
 609                 /* Allocate framebuffer memory: size = modes xy *4 */
 610                 fbi->fb_size = info->var.xres_virtual * info->var.yres_virtual
 611                                 * info->var.bits_per_pixel / 8;
 612         } else {
 613                 fbi->fb_size = MMPFB_DEFAULT_SIZE;
 614         }
 615
 616         fbi->fb_start = dma_alloc_coherent(&pdev->dev, PAGE_ALIGN(fbi->fb_size),
 617                                 &fbi->fb_start_dma, GFP_KERNEL);
 618         if (fbi->fb_start == NULL) {
 619                 dev_err(&pdev->dev, "can't alloc framebuffer\n");
 620                 ret = -ENOMEM;
 621                 goto failed_destroy_mutex;
 622         }
 623         memset(fbi->fb_start, 0, fbi->fb_size);
 624         dev_info(fbi->dev, "fb %dk allocated\n", fbi->fb_size/1024);
 625
 626         /* fb power on */
 627         if (modes_num > 0)
 628                 mmpfb_power(fbi, 1);
 629
 630         ret = fb_info_setup(info, fbi);
 631         if (ret < 0)
 632                 goto failed_free_buff;
 633
 634         ret = register_framebuffer(info);
 635         if (ret < 0) {
 636                 dev_err(&pdev->dev, "Failed to register fb: %d\n", ret);
 637                 ret = -ENXIO;
 638                 goto failed_clear_info;
 639         }
 640
 641         dev_info(fbi->dev, "loaded to /dev/fb%d <%s>.\n",
 642                 info->node, info->fix.id);
 643
 644 #ifdef CONFIG_LOGO
 645         if (fbi->fb_start) {
 646                 fb_prepare_logo(info, 0);
 647                 fb_show_logo(info, 0);
 648         }
 649 #endif
 650
 651         return 0;
 652
 653 failed_clear_info:
 654         fb_info_clear(info);
 655 failed_free_buff:
 656         dma_free_coherent(&pdev->dev, PAGE_ALIGN(fbi->fb_size), fbi->fb_start,
 657                 fbi->fb_start_dma);
 658 failed_destroy_mutex:
 659         mutex_destroy(&fbi->access_ok);
 660 failed:
 661         dev_err(fbi->dev, "mmp-fb: frame buffer device init failed\n");
 662
 663         framebuffer_release(info);
 664
 665         return ret;
 666 }
 667
 668 static struct platform_driver mmpfb_driver = {
 669         .driver         = {
 670                 .name   = "mmp-fb",
 671                 .owner  = THIS_MODULE,
 672         },
 673         .probe          = mmpfb_probe,
 674 };
 675
 676 static int mmpfb_init(void)
 677 {
 678         return platform_driver_register(&mmpfb_driver);
 679 }
 680 module_init(mmpfb_init);
 681
 682 MODULE_AUTHOR("Zhou Zhu <zhou.zhu@marvell.com>");
 683 MODULE_DESCRIPTION("Framebuffer driver for Marvell displays");
 684 MODULE_LICENSE("GPL");