drivers/video/tegra/fb.c

   1 /*
   2  * drivers/video/tegra/fb.c
   3  *
   4  * Copyright (C) 2010 Google, Inc.
   5  * Author: Erik Gilling <konkers@android.com>
   6  *         Colin Cross <ccross@android.com>
   7  *         Travis Geiselbrecht <travis@palm.com>
   8  *
   9  * Copyright (c) 2010-2015, NVIDIA CORPORATION, All rights reserved.
  10  *
  11  * This software is licensed under the terms of the GNU General Public
  12  * License version 2, as published by the Free Software Foundation, and
  13  * may be copied, distributed, and modified under those terms.
  14  *
  15  * This program is distributed in the hope that it will be useful,
  16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  * GNU General Public License for more details.
  19  *
  20  */
  21
  22 #include <linux/fb.h>
  23 #include <linux/module.h>
  24 #include <linux/kernel.h>
  25 #include <linux/errno.h>
  26 #include <linux/string.h>
  27 #include <linux/mm.h>
  28 #include <linux/uaccess.h>
  29 #include <linux/slab.h>
  30 #include <linux/file.h>
  31 #include <linux/workqueue.h>
  32 #include <linux/console.h>
  33
  34 #include <asm/atomic.h>
  35
  36 #include <video/tegrafb.h>
  37
  38 #include <mach/dc.h>
  39 #include <mach/fb.h>
  40 #include <linux/nvhost.h>
  41 #include <linux/nvmap.h>
  42
  43 #include "host/dev.h"
  44 #include "dc/dc_priv.h"
  45
  46 /* Pad pitch to 256-byte boundary. */
  47 #define TEGRA_LINEAR_PITCH_ALIGNMENT 256
  48
  49 #ifdef CONFIG_COMPAT
  50 #define user_ptr(p) ((void __user *)(__u64)(p))
  51 #else
  52 #define user_ptr(p) (p)
  53 #endif
  54
  55 /* palette array used by the fbcon */
  56 static u32 pseudo_palette[16];
  57
  58 static int tegra_fb_check_var(struct fb_var_screeninfo *var,
  59                               struct fb_info *info)
  60 {
  61         struct tegra_fb_info *tegra_fb = info->par;
  62         struct tegra_dc *dc = tegra_fb->win.dc;
  63         struct tegra_dc_out_ops *ops = dc->out_ops;
  64         struct fb_videomode mode;
  65
  66         if ((var->yres * var->xres * var->bits_per_pixel / 8 * 2) >
  67             info->screen_size)
  68                 return -EINVAL;
  69
  70         /* Apply mode filter for HDMI only -LVDS supports only fix mode */
  71         if (ops && ops->mode_filter) {
  72                 /* xoffset and yoffset are not preserved by conversion
  73                  * to fb_videomode */
  74                 __u32 xoffset = var->xoffset;
  75                 __u32 yoffset = var->yoffset;
  76
  77                 fb_var_to_videomode(&mode, var);
  78                 if (!ops->mode_filter(dc, &mode))
  79                         return -EINVAL;
  80
  81                 /* Mode filter may have modified the mode */
  82                 fb_videomode_to_var(var, &mode);
  83
  84                 var->xoffset = xoffset;
  85                 var->yoffset = yoffset;
  86         }
  87
  88         /* Double yres_virtual to allow double buffering through pan_display */
  89         var->yres_virtual = var->yres * 2;
  90
  91         return 0;
  92 }
  93
  94 static int tegra_fb_set_par(struct fb_info *info)
  95 {
  96         struct tegra_fb_info *tegra_fb = info->par;
  97         struct fb_var_screeninfo *var = &info->var;
  98         struct tegra_dc *dc = tegra_fb->win.dc;
  99
 100         if (var->bits_per_pixel) {
 101                 /* we only support RGB ordering for now */
 102                 switch (var->bits_per_pixel) {
 103                 case 32:
 104                         var->red.offset = 0;
 105                         var->red.length = 8;
 106                         var->green.offset = 8;
 107                         var->green.length = 8;
 108                         var->blue.offset = 16;
 109                         var->blue.length = 8;
 110                         var->transp.offset = 24;
 111                         var->transp.length = 8;
 112                         tegra_fb->win.fmt = TEGRA_WIN_FMT_R8G8B8A8;
 113                         break;
 114                 case 16:
 115                         var->red.offset = 11;
 116                         var->red.length = 5;
 117                         var->green.offset = 5;
 118                         var->green.length = 6;
 119                         var->blue.offset = 0;
 120                         var->blue.length = 5;
 121                         tegra_fb->win.fmt = TEGRA_WIN_FMT_B5G6R5;
 122                         break;
 123
 124                 default:
 125                         return -EINVAL;
 126                 }
 127                 /* if line_length unset, then pad the stride */
 128                 if (!info->fix.line_length) {
 129                         info->fix.line_length = var->xres * var->bits_per_pixel
 130                                 / 8;
 131                         info->fix.line_length = round_up(info->fix.line_length,
 132                                                 TEGRA_LINEAR_PITCH_ALIGNMENT);
 133                 }
 134                 tegra_fb->win.stride = info->fix.line_length;
 135                 tegra_fb->win.stride_uv = 0;
 136                 tegra_fb->win.phys_addr_u = 0;
 137                 tegra_fb->win.phys_addr_v = 0;
 138         }
 139
 140         if (var->pixclock) {
 141                 bool stereo;
 142                 unsigned old_len = 0;
 143                 struct fb_videomode m;
 144                 struct fb_videomode *old_mode = NULL;
 145                 struct tegra_fb_info *tegra_fb = info->par;
 146
 147
 148                 fb_var_to_videomode(&m, var);
 149
 150                 /* Load framebuffer info with new mode details*/
 151                 old_mode = info->mode;
 152                 old_len  = info->fix.line_length;
 153                 memcpy(&tegra_fb->mode, &m, sizeof(tegra_fb->mode));
 154                 info->mode = (struct fb_videomode *)&tegra_fb->mode;
 155                 if (!info->mode) {
 156                         dev_warn(&tegra_fb->ndev->dev, "can't match video mode\n");
 157                         info->mode = old_mode;
 158                         return -EINVAL;
 159                 }
 160
 161                 /* Update fix line_length and window stride as per new mode */
 162                 info->fix.line_length = var->xres * var->bits_per_pixel / 8;
 163                 info->fix.line_length = round_up(info->fix.line_length,
 164                         TEGRA_LINEAR_PITCH_ALIGNMENT);
 165                 tegra_fb->win.stride = info->fix.line_length;
 166
 167                 /*
 168                  * only enable stereo if the mode supports it and
 169                  * client requests it
 170                  */
 171                 stereo = !!(var->vmode & info->mode->vmode &
 172 #ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
 173                                         FB_VMODE_STEREO_FRAME_PACK);
 174 #else
 175                                         FB_VMODE_STEREO_LEFT_RIGHT);
 176 #endif
 177
 178                 /* Configure DC with new mode */
 179                 if (tegra_dc_set_fb_mode(dc, info->mode, stereo)) {
 180                         /* Error while configuring DC, fallback to old mode */
 181                         dev_warn(&tegra_fb->ndev->dev, "can't configure dc with mode %ux%u\n",
 182                                 info->mode->xres, info->mode->yres);
 183                         info->mode = old_mode;
 184                         info->fix.line_length = old_len;
 185                         tegra_fb->win.stride = old_len;
 186                         return -EINVAL;
 187                 }
 188
 189                 tegra_fb->win.w.full = dfixed_const(info->mode->xres);
 190                 tegra_fb->win.h.full = dfixed_const(info->mode->yres);
 191                 tegra_fb->win.out_w = info->mode->xres;
 192                 tegra_fb->win.out_h = info->mode->yres;
 193         }
 194         return 0;
 195 }
 196
 197 static int tegra_fb_setcolreg(unsigned regno, unsigned red, unsigned green,
 198         unsigned blue, unsigned transp, struct fb_info *info)
 199 {
 200         struct fb_var_screeninfo *var = &info->var;
 201
 202         if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 203             info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
 204                 u32 v;
 205
 206                 if (regno >= 16)
 207                         return -EINVAL;
 208
 209                 red = (red >> (16 - info->var.red.length));
 210                 green = (green >> (16 - info->var.green.length));
 211                 blue = (blue >> (16 - info->var.blue.length));
 212
 213                 v = (red << var->red.offset) |
 214                         (green << var->green.offset) |
 215                         (blue << var->blue.offset);
 216
 217                 ((u32 *)info->pseudo_palette)[regno] = v;
 218         }
 219
 220         return 0;
 221 }
 222
 223
 224 static int tegra_fb_setcmap(struct fb_cmap *cmap, struct fb_info *info)
 225 {
 226         struct tegra_fb_info *tegra_fb = info->par;
 227         struct tegra_dc *dc = tegra_fb->win.dc;
 228         int i;
 229         u16 *red = cmap->red;
 230         u16 *green = cmap->green;
 231         u16 *blue = cmap->blue;
 232         int start = cmap->start;
 233
 234         if (((unsigned)start > 255) || ((start + cmap->len) > 256))
 235                 return -EINVAL;
 236
 237         if (info->fix.visual == FB_VISUAL_TRUECOLOR ||
 238                 info->fix.visual == FB_VISUAL_DIRECTCOLOR) {
 239                 /*
 240                  * For now we are considering color schemes with
 241                  * cmap->len <=16 as special case of basic color
 242                  * scheme to support fbconsole.But for DirectColor
 243                  * visuals(like the one we actually have, that include
 244                  * a HW LUT),the way it's intended to work is that the
 245                  * actual LUT HW is programmed to the intended values,
 246                  * even for small color maps like those with 16 or fewer
 247                  * entries. The pseudo_palette is then programmed to the
 248                  * identity transform.
 249                  */
 250                 if (cmap->len <= 16) {
 251                         /* Low-color schemes like fbconsole*/
 252                         u16 *transp = cmap->transp;
 253                         u_int vtransp = 0xffff;
 254
 255                         for (i = 0; i < cmap->len; i++) {
 256                                 if (transp)
 257                                         vtransp = *transp++;
 258                                 if (tegra_fb_setcolreg(start++, *red++,
 259                                         *green++, *blue++,
 260                                         vtransp, info))
 261                                                 return -EINVAL;
 262                         }
 263                 } else {
 264                         /* High-color schemes*/
 265                         for (i = 0; i < cmap->len; i++) {
 266                                 dc->fb_lut.r[start+i] = *red++ >> 8;
 267                                 dc->fb_lut.g[start+i] = *green++ >> 8;
 268                                 dc->fb_lut.b[start+i] = *blue++ >> 8;
 269                         }
 270                         tegra_dc_update_lut(dc, -1, -1);
 271                 }
 272         }
 273         return 0;
 274 }
 275
 276 static int tegra_fb_blank(int blank, struct fb_info *info)
 277 {
 278         struct tegra_fb_info *tegra_fb = info->par;
 279         struct tegra_dc *dc = tegra_fb->win.dc;
 280
 281         switch (blank) {
 282         case FB_BLANK_UNBLANK:
 283                 dev_dbg(&tegra_fb->ndev->dev, "unblank\n");
 284                 tegra_dc_enable(dc);
 285                 if (!dc->suspended && dc->blanked &&
 286                     !tegra_dc_restore(dc)) {
 287                         struct tegra_dc_win *win = &tegra_fb->win;
 288                         tegra_dc_update_windows(&win, 1, NULL);
 289                         tegra_dc_sync_windows(&win, 1);
 290                         tegra_dc_program_bandwidth(dc, true);
 291                 }
 292
 293                 dc->blanked = false;
 294                 return 0;
 295
 296         case FB_BLANK_NORMAL:
 297                 dev_dbg(&tegra_fb->ndev->dev, "blank - normal\n");
 298                 /* To pan fb at the unblank */
 299                 if (dc->enabled)
 300                         tegra_fb->curr_xoffset = -1;
 301                 dc->blanked = true;
 302                 tegra_dc_blank(dc, BLANK_ALL);
 303                 return 0;
 304
 305         case FB_BLANK_VSYNC_SUSPEND:
 306         case FB_BLANK_HSYNC_SUSPEND:
 307         case FB_BLANK_POWERDOWN:
 308                 dev_dbg(&tegra_fb->ndev->dev, "blank - powerdown\n");
 309                 /* To pan fb while switching from X */
 310                 if (!dc->suspended && dc->enabled)
 311                         tegra_fb->curr_xoffset = -1;
 312                 tegra_dc_disable(dc);
 313                 return 0;
 314
 315         default:
 316                 return -ENOTTY;
 317         }
 318 }
 319
 320 static int tegra_fb_pan_display(struct fb_var_screeninfo *var,
 321                                 struct fb_info *info)
 322 {
 323         struct tegra_fb_info *tegra_fb = info->par;
 324         char __iomem *flush_start;
 325         char __iomem *flush_end;
 326         phys_addr_t    addr;
 327
 328         /*
 329          * Do nothing if display parameters are same as current values.
 330          */
 331 #if defined(CONFIG_ANDROID)
 332         if ((var->xoffset == tegra_fb->curr_xoffset) &&
 333             (var->yoffset == tegra_fb->curr_yoffset) &&
 334             !(var->activate & FB_ACTIVATE_FORCE))
 335                 return 0;
 336 #endif
 337
 338         if (!tegra_fb->win.cur_handle) {
 339                 flush_start = info->screen_base +
 340                 (var->yoffset * info->fix.line_length);
 341                 flush_end = flush_start + (var->yres * info->fix.line_length);
 342
 343                 info->var.xoffset = var->xoffset;
 344                 info->var.yoffset = var->yoffset;
 345                 /*
 346                  * Save previous values of xoffset and yoffset so we can
 347                  * pan display only when needed.
 348                  */
 349                 tegra_fb->curr_xoffset = var->xoffset;
 350                 tegra_fb->curr_yoffset = var->yoffset;
 351
 352                 addr = tegra_fb->phys_start + (var->yoffset * info->fix.line_length) +
 353                         (var->xoffset * (var->bits_per_pixel/8));
 354
 355                 tegra_fb->win.phys_addr = addr;
 356                 tegra_fb->win.flags = TEGRA_WIN_FLAG_ENABLED;
 357                 tegra_fb->win.flags |= TEGRA_WIN_FLAG_FB;
 358                 tegra_fb->win.virt_addr = info->screen_base;
 359
 360                 if (!tegra_fb->win.dc->suspended) {
 361                         struct tegra_dc_win *win = &tegra_fb->win;
 362                         tegra_dc_update_windows(&win, 1, NULL);
 363                         tegra_dc_sync_windows(&win, 1);
 364                         tegra_dc_program_bandwidth(win->dc, true);
 365                 }
 366         }
 367
 368         return 0;
 369 }
 370
 371 static void tegra_fb_fillrect(struct fb_info *info,
 372                               const struct fb_fillrect *rect)
 373 {
 374         cfb_fillrect(info, rect);
 375 }
 376
 377 static void tegra_fb_copyarea(struct fb_info *info,
 378                               const struct fb_copyarea *region)
 379 {
 380         cfb_copyarea(info, region);
 381 }
 382
 383 static void tegra_fb_imageblit(struct fb_info *info,
 384                                const struct fb_image *image)
 385 {
 386         cfb_imageblit(info, image);
 387 }
 388
 389 static int tegra_get_modedb(struct tegra_dc *dc, struct tegra_fb_modedb *modedb,
 390         struct fb_info *info)
 391 {
 392         unsigned i;
 393         struct fb_var_screeninfo *modedb_ptr;
 394         struct fb_modelist *modelist;
 395
 396         i = 0;
 397         modedb_ptr = user_ptr(modedb->modedb);
 398         list_for_each_entry(modelist, &info->modelist, list) {
 399                 struct fb_var_screeninfo var;
 400
 401                 /* fb_videomode_to_var doesn't fill out all the members
 402                    of fb_var_screeninfo */
 403                 memset(&var, 0x0, sizeof(var));
 404
 405                 fb_videomode_to_var(&var, &modelist->mode);
 406                 var.width = tegra_dc_get_out_width(dc);
 407                 var.height = tegra_dc_get_out_height(dc);
 408
 409                 if (i < modedb->modedb_len) {
 410                         void __user *ptr = &modedb_ptr[i];
 411
 412                         if (copy_to_user(ptr, &var, sizeof(var)))
 413                                 return -EFAULT;
 414                 }
 415                 i++;
 416
 417                 if (var.vmode & FB_VMODE_STEREO_MASK) {
 418                         if (i < modedb->modedb_len) {
 419                                 void __user *ptr = &modedb_ptr[i];
 420
 421                                 var.vmode &= ~FB_VMODE_STEREO_MASK;
 422                                 if (copy_to_user(ptr,
 423                                         &var, sizeof(var)))
 424                                         return -EFAULT;
 425                         }
 426                         i++;
 427                 }
 428         }
 429
 430         /*
 431          * If modedb_len == 0, return how many modes are
 432          * available; otherwise, return how many modes were written.
 433          */
 434         if (modedb->modedb_len == 0)
 435                 modedb->modedb_len = i;
 436         else
 437                 modedb->modedb_len = min(modedb->modedb_len, i);
 438
 439         return 0;
 440 }
 441
 442 static int tegra_fb_ioctl(struct fb_info *info,
 443         unsigned int cmd, unsigned long arg)
 444 {
 445         int res;
 446         struct tegra_fb_info *tegra_fb = (struct tegra_fb_info *)info->par;
 447         struct tegra_dc *dc = tegra_fb->win.dc;
 448         struct tegra_fb_modedb modedb;
 449         struct fb_vblank vblank = {};
 450
 451         switch (cmd) {
 452 #ifdef CONFIG_COMPAT
 453         case FBIO_TEGRA_GET_MODEDB_COMPAT: {
 454                 struct tegra_fb_modedb_compat modedb_compat;
 455
 456                 if (copy_from_user(&modedb_compat, (void __user *)arg,
 457                         sizeof(modedb_compat)))
 458                         return -EFAULT;
 459                 /* convert compat version to full version */
 460                 modedb.modedb = (void __user *)modedb_compat.modedb;
 461                 modedb.modedb_len = modedb_compat.modedb_len;
 462
 463                 res = tegra_get_modedb(dc, &modedb, info);
 464                 if (res)
 465                         return res;
 466
 467                 /* convert full version back to compat version */
 468                 modedb_compat.modedb_len = modedb.modedb_len;
 469                 if (copy_to_user((void __user *)arg, &modedb_compat,
 470                         sizeof(modedb_compat)))
 471                         return -EFAULT;
 472                 break;
 473         }
 474 #endif
 475         case FBIO_TEGRA_GET_MODEDB:
 476                 if (copy_from_user(&modedb, (void __user *)arg, sizeof(modedb)))
 477                         return -EFAULT;
 478
 479                 res = tegra_get_modedb(dc, &modedb, info);
 480                 if (res)
 481                         return res;
 482
 483                 if (copy_to_user((void __user *)arg, &modedb, sizeof(modedb)))
 484                         return -EFAULT;
 485                 break;
 486
 487         case FBIOGET_VBLANK:
 488                 if (tegra_dc_has_vsync(dc))
 489                         vblank.flags = FB_VBLANK_HAVE_VSYNC;
 490
 491                 if (copy_to_user(
 492                         (void __user *)arg, &vblank, sizeof(vblank)))
 493                         return -EFAULT;
 494                 break;
 495
 496         case FBIO_WAITFORVSYNC:
 497                 return tegra_dc_wait_for_vsync(dc);
 498
 499         default:
 500                 return -ENOTTY;
 501         }
 502
 503         return 0;
 504 }
 505
 506 int tegra_fb_get_mode(struct tegra_dc *dc) {
 507         if (!dc->fb->info->mode)
 508                 return -1;
 509         return dc->fb->info->mode->refresh;
 510 }
 511
 512 int tegra_fb_set_mode(struct tegra_dc *dc, int fps) {
 513         size_t stereo;
 514         struct list_head *pos;
 515         struct fb_videomode *best_mode = NULL;
 516         int curr_diff = INT_MAX; /* difference of best_mode refresh rate */
 517         struct fb_modelist *modelist;
 518         struct fb_info *info = dc->fb->info;
 519
 520         list_for_each(pos, &info->modelist) {
 521                 struct fb_videomode *mode;
 522
 523                 modelist = list_entry(pos, struct fb_modelist, list);
 524                 mode = &modelist->mode;
 525                 if (fps <= mode->refresh && curr_diff > (mode->refresh - fps)) {
 526                         curr_diff = mode->refresh - fps;
 527                         best_mode = mode;
 528                 }
 529         }
 530         if (best_mode) {
 531                 info->mode = best_mode;
 532                 stereo = !!(info->var.vmode & info->mode->vmode &
 533 #ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
 534                                 FB_VMODE_STEREO_FRAME_PACK);
 535 #else
 536                                 FB_VMODE_STEREO_LEFT_RIGHT);
 537 #endif
 538                 return tegra_dc_set_fb_mode(dc, best_mode, stereo);
 539         }
 540         return -EIO;
 541 }
 542
 543 static struct fb_ops tegra_fb_ops = {
 544         .owner = THIS_MODULE,
 545         .fb_check_var = tegra_fb_check_var,
 546         .fb_set_par = tegra_fb_set_par,
 547         .fb_setcmap = tegra_fb_setcmap,
 548         .fb_blank = tegra_fb_blank,
 549         .fb_pan_display = tegra_fb_pan_display,
 550         .fb_fillrect = tegra_fb_fillrect,
 551         .fb_copyarea = tegra_fb_copyarea,
 552         .fb_imageblit = tegra_fb_imageblit,
 553         .fb_ioctl = tegra_fb_ioctl,
 554 #ifdef CONFIG_COMPAT
 555         .fb_compat_ioctl = tegra_fb_ioctl,
 556 #endif
 557 };
 558
 559 /* Enabling the pan_display by resetting the cache of offset */
 560 void tegra_fb_pan_display_reset(struct tegra_fb_info *fb_info)
 561 {
 562         fb_info->curr_xoffset = -1;
 563 }
 564
 565 void tegra_fb_update_monspecs(struct tegra_fb_info *fb_info,
 566                               struct fb_monspecs *specs,
 567                               bool (*mode_filter)(const struct tegra_dc *dc,
 568                                                   struct fb_videomode *mode))
 569 {
 570         struct fb_event event;
 571         int i;
 572
 573         mutex_lock(&fb_info->info->lock);
 574         fb_destroy_modedb(fb_info->info->monspecs.modedb);
 575
 576         fb_destroy_modelist(&fb_info->info->modelist);
 577
 578         /* Notify layers above fb.c that the hardware is unavailable */
 579         fb_info->info->state = FBINFO_STATE_SUSPENDED;
 580
 581         if (specs == NULL) {
 582                 struct tegra_dc_mode mode;
 583                 memset(&fb_info->info->monspecs, 0x0,
 584                        sizeof(fb_info->info->monspecs));
 585                 memset(&mode, 0x0, sizeof(mode));
 586
 587                 /*
 588                  * reset video mode properties to prevent garbage being
 589                  * displayed on 'mode' device.
 590                  */
 591                 fb_info->info->mode = (struct fb_videomode*) NULL;
 592                 fb_add_videomode(&tegra_dc_vga_mode, &fb_info->info->modelist);
 593                 fb_videomode_to_var(&fb_info->info->var, &tegra_dc_vga_mode);
 594
 595                 tegra_dc_set_mode(fb_info->win.dc, &mode);
 596                 mutex_unlock(&fb_info->info->lock);
 597                 return;
 598         }
 599
 600         memcpy(&fb_info->info->monspecs, specs,
 601                sizeof(fb_info->info->monspecs));
 602         fb_info->info->mode = specs->modedb;
 603
 604         for (i = 0; i < specs->modedb_len; i++) {
 605                 if (mode_filter) {
 606                         if (mode_filter(fb_info->win.dc, &specs->modedb[i]))
 607                                 fb_add_videomode(&specs->modedb[i],
 608                                                  &fb_info->info->modelist);
 609                 } else {
 610                         fb_add_videomode(&specs->modedb[i],
 611                                          &fb_info->info->modelist);
 612                 }
 613         }
 614
 615         event.info = fb_info->info;
 616         /* Restoring to state running. */
 617         fb_info->info->state =  FBINFO_STATE_RUNNING;
 618 #ifdef CONFIG_FRAMEBUFFER_CONSOLE
 619         console_lock();
 620         fb_notifier_call_chain(FB_EVENT_NEW_MODELIST, &event);
 621         /* When we can't satify the requested mode, we will fall back
 622          to default mode */
 623         if (PICOS2KHZ(specs->modedb[0].pixclock) >
 624                 PICOS2KHZ(tegra_dc_get_out_max_pixclock(fb_info->win.dc))) {
 625                 /* Program DC with default mode */
 626                 tegra_dc_set_fb_mode(fb_info->win.dc,
 627                                 &tegra_dc_vga_mode, false);
 628                 fb_videomode_to_var(&fb_info->info->var, &tegra_dc_vga_mode);
 629         } else {
 630                 /* Program DC with first mode */
 631                 tegra_dc_set_fb_mode(fb_info->win.dc, specs->modedb, false);
 632                 fb_videomode_to_var(&fb_info->info->var, &specs->modedb[0]);
 633         }
 634         fb_notifier_call_chain(FB_EVENT_MODE_CHANGE_ALL, &event);
 635         console_unlock();
 636 #else
 637         fb_notifier_call_chain(FB_EVENT_NEW_MODELIST, &event);
 638 #endif
 639         mutex_unlock(&fb_info->info->lock);
 640 }
 641
 642 static ssize_t nvdps_show(struct device *device,
 643         struct device_attribute *attr, char *buf)
 644 {
 645         int refresh_rate;
 646         struct platform_device *ndev = to_platform_device(device);
 647         struct tegra_dc *dc = platform_get_drvdata(ndev);
 648
 649         refresh_rate = tegra_fb_get_mode(dc);
 650         return snprintf(buf, PAGE_SIZE, "%d\n", refresh_rate);
 651 }
 652
 653
 654 static ssize_t nvdps_store(struct device *dev,
 655         struct device_attribute *attr, const char *buf, size_t count)
 656 {
 657         struct platform_device *ndev = to_platform_device(dev);
 658         struct tegra_dc *dc = platform_get_drvdata(ndev);
 659         int refresh_rate;
 660         int e;
 661
 662         e = kstrtoint(buf, 10, &refresh_rate);
 663         if (e)
 664                 return e;
 665         e = tegra_fb_set_mode(dc, refresh_rate);
 666         if (e)
 667                 return e;
 668
 669         return count;
 670 }
 671
 672 static DEVICE_ATTR(nvdps, S_IRUGO|S_IWUSR, nvdps_show, nvdps_store);
 673
 674
 675 int tegra_fb_create_sysfs(struct device *dev)
 676 {
 677         return device_create_file(dev, &dev_attr_nvdps);
 678 }
 679
 680 void tegra_fb_remove_sysfs(struct device *dev)
 681 {
 682         device_remove_file(dev, &dev_attr_nvdps);
 683 }
 684
 685 struct tegra_fb_info *tegra_fb_register(struct platform_device *ndev,
 686                                         struct tegra_dc *dc,
 687                                         struct tegra_fb_data *fb_data,
 688                                         struct resource *fb_mem)
 689 {
 690         struct fb_info *info;
 691         struct tegra_fb_info *tegra_fb;
 692         void __iomem *fb_base = NULL;
 693         phys_addr_t fb_size = 0;
 694         int ret = 0;
 695         int mode_idx;
 696         unsigned stride;
 697         struct fb_videomode m;
 698
 699         if (!tegra_dc_get_window(dc, fb_data->win)) {
 700                 dev_err(&ndev->dev, "dc does not have a window at index %d\n",
 701                         fb_data->win);
 702                 return ERR_PTR(-ENOENT);
 703         }
 704
 705         info = framebuffer_alloc(sizeof(struct tegra_fb_info), &ndev->dev);
 706         if (!info) {
 707                 ret = -ENOMEM;
 708                 goto err;
 709         }
 710
 711         tegra_fb = info->par;
 712         tegra_fb->ndev = ndev;
 713         tegra_fb->fb_mem = fb_mem;
 714         tegra_fb->xres = fb_data->xres;
 715         tegra_fb->yres = fb_data->yres;
 716
 717         tegra_fb->win.idx = fb_data->win;
 718
 719         if (fb_mem) {
 720                 fb_size = resource_size(fb_mem);
 721                 tegra_fb->phys_start = fb_mem->start;
 722                 fb_base = ioremap_wc(tegra_fb->phys_start, fb_size);
 723                 if (!fb_base) {
 724                         dev_err(&ndev->dev, "fb can't be mapped\n");
 725                         ret = -EBUSY;
 726                         goto err_free;
 727                 }
 728                 tegra_fb->valid = true;
 729         }
 730
 731         info->fix.line_length = fb_data->xres * fb_data->bits_per_pixel / 8;
 732
 733         stride = tegra_dc_get_stride(dc, 0);
 734         if (!stride) /* default to pad the stride */
 735                 stride = round_up(info->fix.line_length,
 736                         TEGRA_LINEAR_PITCH_ALIGNMENT);
 737
 738         info->fbops = &tegra_fb_ops;
 739         info->pseudo_palette = pseudo_palette;
 740         info->screen_base = fb_base;
 741         info->screen_size = fb_size;
 742
 743         strlcpy(info->fix.id, "tegra_fb", sizeof(info->fix.id));
 744         info->fix.type          = FB_TYPE_PACKED_PIXELS;
 745         info->fix.visual        = FB_VISUAL_TRUECOLOR;
 746         info->fix.xpanstep      = 1;
 747         info->fix.ypanstep      = 1;
 748         info->fix.accel         = FB_ACCEL_NONE;
 749         /* Note:- Use tegra_fb_info.phys_start instead of
 750          *        fb_info.fix->smem_start when LPAE is enabled. */
 751         info->fix.smem_start    = (u32)tegra_fb->phys_start;
 752         info->fix.smem_len      = fb_size;
 753         info->fix.line_length = stride;
 754         INIT_LIST_HEAD(&info->modelist);
 755         /* pick first mode as the default for initialization */
 756         tegra_dc_to_fb_videomode(&m, &dc->mode);
 757         fb_videomode_to_var(&info->var, &m);
 758         info->var.xres_virtual          = fb_data->xres;
 759         info->var.yres_virtual          = fb_data->yres * 2;
 760         info->var.bits_per_pixel        = fb_data->bits_per_pixel;
 761         info->var.activate              = FB_ACTIVATE_VBL;
 762         info->var.height                = tegra_dc_get_out_height(dc);
 763         info->var.width                 = tegra_dc_get_out_width(dc);
 764
 765         tegra_fb->win.dc = dc;
 766         tegra_fb->win.x.full = dfixed_const(0);
 767         tegra_fb->win.y.full = dfixed_const(0);
 768         tegra_fb->win.w.full = dfixed_const(fb_data->xres);
 769         tegra_fb->win.h.full = dfixed_const(fb_data->yres);
 770         /* TODO: set to output res dc */
 771         tegra_fb->win.out_x = 0;
 772         tegra_fb->win.out_y = 0;
 773         tegra_fb->win.out_w = fb_data->xres;
 774         tegra_fb->win.out_h = fb_data->yres;
 775         tegra_fb->win.z = 0;
 776         tegra_fb->win.phys_addr = tegra_fb->phys_start;
 777         tegra_fb->win.virt_addr = fb_base;
 778         tegra_fb->win.phys_addr_u = 0;
 779         tegra_fb->win.phys_addr_v = 0;
 780         tegra_fb->win.stride = info->fix.line_length;
 781         tegra_fb->win.stride_uv = 0;
 782         tegra_fb->win.flags = TEGRA_WIN_FLAG_ENABLED;
 783         tegra_fb->win.global_alpha = 0xFF;
 784
 785         for (mode_idx = 0; mode_idx < dc->out->n_modes; mode_idx++) {
 786                 struct tegra_dc_mode mode = dc->out->modes[mode_idx];
 787                 struct fb_videomode vmode;
 788
 789                 mode.pclk = dc->mode.pclk;
 790
 791                 if (mode.pclk > 1000) {
 792                         tegra_dc_to_fb_videomode(&vmode, &mode);
 793                         fb_add_videomode(&vmode, &info->modelist);
 794                 }
 795         }
 796
 797         if (fb_mem)
 798                 tegra_fb_set_par(info);
 799
 800         if (register_framebuffer(info)) {
 801                 dev_err(&ndev->dev, "failed to register framebuffer\n");
 802                 ret = -ENODEV;
 803                 goto err_iounmap_fb;
 804         }
 805
 806         tegra_fb->info = info;
 807
 808         if (fb_data->flags & TEGRA_FB_FLIP_ON_PROBE) {
 809                 struct tegra_dc_win *win = &tegra_fb->win;
 810                 tegra_dc_update_windows(&win, 1, NULL);
 811                 tegra_dc_sync_windows(&win, 1);
 812                 tegra_dc_program_bandwidth(win->dc, true);
 813         }
 814
 815         dev_info(&ndev->dev, "fb registered\n");
 816
 817         return tegra_fb;
 818
 819 err_iounmap_fb:
 820         if (fb_base)
 821                 iounmap(fb_base);
 822 err_free:
 823         framebuffer_release(info);
 824 err:
 825         return ERR_PTR(ret);
 826 }
 827
 828 void tegra_fb_unregister(struct tegra_fb_info *fb_info)
 829 {
 830         struct fb_info *info = fb_info->info;
 831
 832         unregister_framebuffer(info);
 833
 834         iounmap(info->screen_base);
 835         framebuffer_release(info);
 836 }