drivers/video/fbdev/mmp/fb/mmpfb.c

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