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