drivers/video/fbdev/bcm2708_fb.c

   1 /*
   2  *  linux/drivers/video/bcm2708_fb.c
   3  *
   4  * Copyright (C) 2010 Broadcom
   5  * Copyright (C) 2018 Raspberry Pi (Trading) Ltd
   6  *
   7  * This file is subject to the terms and conditions of the GNU General Public
   8  * License.  See the file COPYING in the main directory of this archive
   9  * for more details.
  10  *
  11  * Broadcom simple framebuffer driver
  12  *
  13  * This file is derived from cirrusfb.c
  14  * Copyright 1999-2001 Jeff Garzik <jgarzik@pobox.com>
  15  *
  16  */
  17
  18 #include <linux/module.h>
  19 #include <linux/kernel.h>
  20 #include <linux/errno.h>
  21 #include <linux/string.h>
  22 #include <linux/slab.h>
  23 #include <linux/mm.h>
  24 #include <linux/fb.h>
  25 #include <linux/init.h>
  26 #include <linux/interrupt.h>
  27 #include <linux/ioport.h>
  28 #include <linux/list.h>
  29 #include <linux/platform_data/dma-bcm2708.h>
  30 #include <linux/platform_device.h>
  31 #include <linux/clk.h>
  32 #include <linux/printk.h>
  33 #include <linux/console.h>
  34 #include <linux/debugfs.h>
  35 #include <linux/uaccess.h>
  36 #include <linux/io.h>
  37 #include <linux/dma-mapping.h>
  38 #include <linux/cred.h>
  39 #include <soc/bcm2835/raspberrypi-firmware.h>
  40 #include <linux/mutex.h>
  41 #include <linux/compat.h>
  42
  43 //#define BCM2708_FB_DEBUG
  44 #define MODULE_NAME "bcm2708_fb"
  45
  46 #ifdef BCM2708_FB_DEBUG
  47 #define print_debug(fmt, ...) pr_debug("%s:%s:%d: " fmt, \
  48                         MODULE_NAME, __func__, __LINE__, ##__VA_ARGS__)
  49 #else
  50 #define print_debug(fmt, ...)
  51 #endif
  52
  53 /* This is limited to 16 characters when displayed by X startup */
  54 static const char *bcm2708_name = "BCM2708 FB";
  55
  56 #define DRIVER_NAME "bcm2708_fb"
  57
  58 static int fbwidth = 800;       /* module parameter */
  59 static int fbheight = 480;      /* module parameter */
  60 static int fbdepth = 32;        /* module parameter */
  61 static int fbswap;              /* module parameter */
  62
  63 static u32 dma_busy_wait_threshold = 1 << 15;
  64 module_param(dma_busy_wait_threshold, int, 0644);
  65 MODULE_PARM_DESC(dma_busy_wait_threshold, "Busy-wait for DMA completion below this area");
  66
  67 struct fb_alloc_tags {
  68         struct rpi_firmware_property_tag_header tag1;
  69         u32 xres, yres;
  70         struct rpi_firmware_property_tag_header tag2;
  71         u32 xres_virtual, yres_virtual;
  72         struct rpi_firmware_property_tag_header tag3;
  73         u32 bpp;
  74         struct rpi_firmware_property_tag_header tag4;
  75         u32 xoffset, yoffset;
  76         struct rpi_firmware_property_tag_header tag5;
  77         u32 base, screen_size;
  78         struct rpi_firmware_property_tag_header tag6;
  79         u32 pitch;
  80 };
  81
  82 struct bcm2708_fb_stats {
  83         struct debugfs_regset32 regset;
  84         u32 dma_copies;
  85         u32 dma_irqs;
  86 };
  87
  88 struct vc4_display_settings_t {
  89         u32 display_num;
  90         u32 width;
  91         u32 height;
  92         u32 depth;
  93         u32 pitch;
  94         u32 virtual_width;
  95         u32 virtual_height;
  96         u32 virtual_width_offset;
  97         u32 virtual_height_offset;
  98         unsigned long fb_bus_address;
  99 };
 100
 101 struct bcm2708_fb_dev;
 102
 103 struct bcm2708_fb {
 104         struct fb_info fb;
 105         struct platform_device *dev;
 106         u32 cmap[16];
 107         u32 gpu_cmap[256];
 108         struct dentry *debugfs_dir;
 109         struct dentry *debugfs_subdir;
 110         unsigned long fb_bus_address;
 111         struct { u32 base, length; } gpu;
 112         struct vc4_display_settings_t display_settings;
 113         struct debugfs_regset32 screeninfo_regset;
 114         struct bcm2708_fb_dev *fbdev;
 115         unsigned int image_size;
 116         dma_addr_t dma_addr;
 117         void *cpuaddr;
 118 };
 119
 120 #define MAX_FRAMEBUFFERS 3
 121
 122 struct bcm2708_fb_dev {
 123         int firmware_supports_multifb;
 124         /* Protects the DMA system from multiple FB access */
 125         struct mutex dma_mutex;
 126         int dma_chan;
 127         int dma_irq;
 128         void __iomem *dma_chan_base;
 129         wait_queue_head_t dma_waitq;
 130         bool disable_arm_alloc;
 131         struct bcm2708_fb_stats dma_stats;
 132         void *cb_base;  /* DMA control blocks */
 133         dma_addr_t cb_handle;
 134         int instance_count;
 135         int num_displays;
 136         struct rpi_firmware *fw;
 137         struct bcm2708_fb displays[MAX_FRAMEBUFFERS];
 138 };
 139
 140 #define to_bcm2708(info)        container_of(info, struct bcm2708_fb, fb)
 141
 142 static void bcm2708_fb_debugfs_deinit(struct bcm2708_fb *fb)
 143 {
 144         debugfs_remove_recursive(fb->debugfs_subdir);
 145         fb->debugfs_subdir = NULL;
 146
 147         fb->fbdev->instance_count--;
 148
 149         if (!fb->fbdev->instance_count) {
 150                 debugfs_remove_recursive(fb->debugfs_dir);
 151                 fb->debugfs_dir = NULL;
 152         }
 153 }
 154
 155 static int bcm2708_fb_debugfs_init(struct bcm2708_fb *fb)
 156 {
 157         char buf[3];
 158         struct bcm2708_fb_dev *fbdev = fb->fbdev;
 159
 160         static struct debugfs_reg32 stats_registers[] = {
 161         {"dma_copies", offsetof(struct bcm2708_fb_stats, dma_copies)},
 162         {"dma_irqs",   offsetof(struct bcm2708_fb_stats, dma_irqs)},
 163         };
 164
 165         static struct debugfs_reg32 screeninfo[] = {
 166         {"width",        offsetof(struct fb_var_screeninfo, xres)},
 167         {"height",       offsetof(struct fb_var_screeninfo, yres)},
 168         {"bpp",          offsetof(struct fb_var_screeninfo, bits_per_pixel)},
 169         {"xres_virtual", offsetof(struct fb_var_screeninfo, xres_virtual)},
 170         {"yres_virtual", offsetof(struct fb_var_screeninfo, yres_virtual)},
 171         {"xoffset",      offsetof(struct fb_var_screeninfo, xoffset)},
 172         {"yoffset",      offsetof(struct fb_var_screeninfo, yoffset)},
 173         };
 174
 175         fb->debugfs_dir = debugfs_lookup(DRIVER_NAME, NULL);
 176
 177         if (!fb->debugfs_dir)
 178                 fb->debugfs_dir = debugfs_create_dir(DRIVER_NAME, NULL);
 179
 180         if (!fb->debugfs_dir) {
 181                 dev_warn(fb->fb.dev, "%s: could not create debugfs folder\n",
 182                          __func__);
 183                 return -EFAULT;
 184         }
 185
 186         snprintf(buf, sizeof(buf), "%u", fb->display_settings.display_num);
 187
 188         fb->debugfs_subdir = debugfs_create_dir(buf, fb->debugfs_dir);
 189
 190         if (!fb->debugfs_subdir) {
 191                 dev_warn(fb->fb.dev, "%s: could not create debugfs entry %u\n",
 192                          __func__, fb->display_settings.display_num);
 193                 return -EFAULT;
 194         }
 195
 196         fbdev->dma_stats.regset.regs = stats_registers;
 197         fbdev->dma_stats.regset.nregs = ARRAY_SIZE(stats_registers);
 198         fbdev->dma_stats.regset.base = &fbdev->dma_stats;
 199
 200         debugfs_create_regset32("dma_stats", 0444, fb->debugfs_subdir,
 201                                 &fbdev->dma_stats.regset);
 202
 203         fb->screeninfo_regset.regs = screeninfo;
 204         fb->screeninfo_regset.nregs = ARRAY_SIZE(screeninfo);
 205         fb->screeninfo_regset.base = &fb->fb.var;
 206
 207         debugfs_create_regset32("screeninfo", 0444, fb->debugfs_subdir,
 208                                 &fb->screeninfo_regset);
 209
 210         fbdev->instance_count++;
 211
 212         return 0;
 213 }
 214
 215 static void set_display_num(struct bcm2708_fb *fb)
 216 {
 217         if (fb && fb->fbdev && fb->fbdev->firmware_supports_multifb) {
 218                 u32 tmp = fb->display_settings.display_num;
 219
 220                 if (rpi_firmware_property(fb->fbdev->fw,
 221                                           RPI_FIRMWARE_FRAMEBUFFER_SET_DISPLAY_NUM,
 222                                           &tmp,
 223                                           sizeof(tmp)))
 224                         dev_warn_once(fb->fb.dev,
 225                                       "Set display number call failed. Old GPU firmware?");
 226         }
 227 }
 228
 229 static int bcm2708_fb_set_bitfields(struct fb_var_screeninfo *var)
 230 {
 231         int ret = 0;
 232
 233         memset(&var->transp, 0, sizeof(var->transp));
 234
 235         var->red.msb_right = 0;
 236         var->green.msb_right = 0;
 237         var->blue.msb_right = 0;
 238
 239         switch (var->bits_per_pixel) {
 240         case 1:
 241         case 2:
 242         case 4:
 243         case 8:
 244                 var->red.length = var->bits_per_pixel;
 245                 var->red.offset = 0;
 246                 var->green.length = var->bits_per_pixel;
 247                 var->green.offset = 0;
 248                 var->blue.length = var->bits_per_pixel;
 249                 var->blue.offset = 0;
 250                 break;
 251         case 16:
 252                 var->red.length = 5;
 253                 var->blue.length = 5;
 254                 /*
 255                  * Green length can be 5 or 6 depending whether
 256                  * we're operating in RGB555 or RGB565 mode.
 257                  */
 258                 if (var->green.length != 5 && var->green.length != 6)
 259                         var->green.length = 6;
 260                 break;
 261         case 24:
 262                 var->red.length = 8;
 263                 var->blue.length = 8;
 264                 var->green.length = 8;
 265                 break;
 266         case 32:
 267                 var->red.length = 8;
 268                 var->green.length = 8;
 269                 var->blue.length = 8;
 270                 var->transp.length = 8;
 271                 break;
 272         default:
 273                 ret = -EINVAL;
 274                 break;
 275         }
 276
 277         /*
 278          * >= 16bpp displays have separate colour component bitfields
 279          * encoded in the pixel data.  Calculate their position from
 280          * the bitfield length defined above.
 281          */
 282         if (ret == 0 && var->bits_per_pixel >= 24 && fbswap) {
 283                 var->blue.offset = 0;
 284                 var->green.offset = var->blue.offset + var->blue.length;
 285                 var->red.offset = var->green.offset + var->green.length;
 286                 var->transp.offset = var->red.offset + var->red.length;
 287         } else if (ret == 0 && var->bits_per_pixel >= 24) {
 288                 var->red.offset = 0;
 289                 var->green.offset = var->red.offset + var->red.length;
 290                 var->blue.offset = var->green.offset + var->green.length;
 291                 var->transp.offset = var->blue.offset + var->blue.length;
 292         } else if (ret == 0 && var->bits_per_pixel >= 16) {
 293                 var->blue.offset = 0;
 294                 var->green.offset = var->blue.offset + var->blue.length;
 295                 var->red.offset = var->green.offset + var->green.length;
 296                 var->transp.offset = var->red.offset + var->red.length;
 297         }
 298
 299         return ret;
 300 }
 301
 302 static int bcm2708_fb_check_var(struct fb_var_screeninfo *var,
 303                                 struct fb_info *info)
 304 {
 305         /* info input, var output */
 306         print_debug("%s(%p) %ux%u (%ux%u), %ul, %u\n",
 307                     __func__, info, info->var.xres, info->var.yres,
 308                     info->var.xres_virtual, info->var.yres_virtual,
 309                     info->screen_size, info->var.bits_per_pixel);
 310         print_debug("%s(%p) %ux%u (%ux%u), %u\n", __func__, var, var->xres,
 311                     var->yres, var->xres_virtual, var->yres_virtual,
 312                     var->bits_per_pixel);
 313
 314         if (!var->bits_per_pixel)
 315                 var->bits_per_pixel = 16;
 316
 317         if (bcm2708_fb_set_bitfields(var) != 0) {
 318                 pr_err("%s: invalid bits_per_pixel %d\n", __func__,
 319                        var->bits_per_pixel);
 320                 return -EINVAL;
 321         }
 322
 323         if (var->xres_virtual < var->xres)
 324                 var->xres_virtual = var->xres;
 325         /* use highest possible virtual resolution */
 326         if (var->yres_virtual == -1) {
 327                 var->yres_virtual = 480;
 328
 329                 pr_err("%s: virtual resolution set to maximum of %dx%d\n",
 330                        __func__, var->xres_virtual, var->yres_virtual);
 331         }
 332         if (var->yres_virtual < var->yres)
 333                 var->yres_virtual = var->yres;
 334
 335         if (var->xoffset < 0)
 336                 var->xoffset = 0;
 337         if (var->yoffset < 0)
 338                 var->yoffset = 0;
 339
 340         /* truncate xoffset and yoffset to maximum if too high */
 341         if (var->xoffset > var->xres_virtual - var->xres)
 342                 var->xoffset = var->xres_virtual - var->xres - 1;
 343         if (var->yoffset > var->yres_virtual - var->yres)
 344                 var->yoffset = var->yres_virtual - var->yres - 1;
 345
 346         return 0;
 347 }
 348
 349 static int bcm2708_fb_set_par(struct fb_info *info)
 350 {
 351         struct bcm2708_fb *fb = to_bcm2708(info);
 352         struct fb_alloc_tags fbinfo = {
 353                 .tag1 = { RPI_FIRMWARE_FRAMEBUFFER_SET_PHYSICAL_WIDTH_HEIGHT,
 354                           8, 0, },
 355                         .xres = info->var.xres,
 356                         .yres = info->var.yres,
 357                 .tag2 = { RPI_FIRMWARE_FRAMEBUFFER_SET_VIRTUAL_WIDTH_HEIGHT,
 358                           8, 0, },
 359                         .xres_virtual = info->var.xres_virtual,
 360                         .yres_virtual = info->var.yres_virtual,
 361                 .tag3 = { RPI_FIRMWARE_FRAMEBUFFER_SET_DEPTH, 4, 0 },
 362                         .bpp = info->var.bits_per_pixel,
 363                 .tag4 = { RPI_FIRMWARE_FRAMEBUFFER_SET_VIRTUAL_OFFSET, 8, 0 },
 364                         .xoffset = info->var.xoffset,
 365                         .yoffset = info->var.yoffset,
 366                 .tag5 = { RPI_FIRMWARE_FRAMEBUFFER_ALLOCATE, 8, 0 },
 367                         /* base and screen_size will be initialised later */
 368                 .tag6 = { RPI_FIRMWARE_FRAMEBUFFER_SET_PITCH, 4, 0 },
 369                         /* pitch will be initialised later */
 370         };
 371         int ret, image_size;
 372
 373         print_debug("%s(%p) %dx%d (%dx%d), %d, %d (display %d)\n", __func__,
 374                     info,
 375                     info->var.xres, info->var.yres, info->var.xres_virtual,
 376                     info->var.yres_virtual, (int)info->screen_size,
 377                     info->var.bits_per_pixel, value);
 378
 379         /* Need to set the display number to act on first
 380          * Cannot do it in the tag list because on older firmware the call
 381          * will fail and stop the rest of the list being executed.
 382          * We can ignore this call failing as the default at other end is 0
 383          */
 384         set_display_num(fb);
 385
 386         /* Try allocating our own buffer. We can specify all the parameters */
 387         image_size = ((info->var.xres * info->var.yres) *
 388                       info->var.bits_per_pixel) >> 3;
 389
 390         if (!fb->fbdev->disable_arm_alloc &&
 391             (image_size != fb->image_size || !fb->dma_addr)) {
 392                 if (fb->dma_addr) {
 393                         dma_free_coherent(info->device, fb->image_size,
 394                                           fb->cpuaddr, fb->dma_addr);
 395                         fb->image_size = 0;
 396                         fb->cpuaddr = NULL;
 397                         fb->dma_addr = 0;
 398                 }
 399
 400                 fb->cpuaddr = dma_alloc_coherent(info->device, image_size,
 401                                                  &fb->dma_addr, GFP_KERNEL);
 402
 403                 if (!fb->cpuaddr) {
 404                         fb->dma_addr = 0;
 405                         fb->fbdev->disable_arm_alloc = true;
 406                 } else {
 407                         fb->image_size = image_size;
 408                 }
 409         }
 410
 411         if (fb->cpuaddr) {
 412                 fbinfo.base = fb->dma_addr;
 413                 fbinfo.screen_size = image_size;
 414                 fbinfo.pitch = (info->var.xres * info->var.bits_per_pixel) >> 3;
 415
 416                 ret = rpi_firmware_property_list(fb->fbdev->fw, &fbinfo,
 417                                                  sizeof(fbinfo));
 418                 if (ret || fbinfo.base != fb->dma_addr) {
 419                         /* Firmware either failed, or assigned a different base
 420                          * address (ie it doesn't support being passed an FB
 421                          * allocation).
 422                          * Destroy the allocation, and don't try again.
 423                          */
 424                         dma_free_coherent(info->device, fb->image_size,
 425                                           fb->cpuaddr, fb->dma_addr);
 426                         fb->image_size = 0;
 427                         fb->cpuaddr = NULL;
 428                         fb->dma_addr = 0;
 429                         fb->fbdev->disable_arm_alloc = true;
 430                 }
 431         } else {
 432                 /* Our allocation failed - drop into the old scheme of
 433                  * allocation by the VPU.
 434                  */
 435                 ret = -ENOMEM;
 436         }
 437
 438         if (ret) {
 439                 /* Old scheme:
 440                  * - FRAMEBUFFER_ALLOCATE passes 0 for base and screen_size.
 441                  * - GET_PITCH instead of SET_PITCH.
 442                  */
 443                 fbinfo.base = 0;
 444                 fbinfo.screen_size = 0;
 445                 fbinfo.tag6.tag = RPI_FIRMWARE_FRAMEBUFFER_GET_PITCH;
 446                 fbinfo.pitch = 0;
 447
 448                 ret = rpi_firmware_property_list(fb->fbdev->fw, &fbinfo,
 449                                                  sizeof(fbinfo));
 450                 if (ret) {
 451                         dev_err(info->device,
 452                                 "Failed to allocate GPU framebuffer (%d)\n",
 453                                 ret);
 454                         return ret;
 455                 }
 456         }
 457
 458         if (info->var.bits_per_pixel <= 8)
 459                 fb->fb.fix.visual = FB_VISUAL_PSEUDOCOLOR;
 460         else
 461                 fb->fb.fix.visual = FB_VISUAL_TRUECOLOR;
 462
 463         fb->fb.fix.line_length = fbinfo.pitch;
 464         fbinfo.base |= 0x40000000;
 465         fb->fb_bus_address = fbinfo.base;
 466         fbinfo.base &= ~0xc0000000;
 467         fb->fb.fix.smem_start = fbinfo.base;
 468         fb->fb.fix.smem_len = fbinfo.pitch * fbinfo.yres_virtual;
 469         fb->fb.screen_size = fbinfo.screen_size;
 470
 471         if (!fb->dma_addr) {
 472                 if (fb->fb.screen_base)
 473                         iounmap(fb->fb.screen_base);
 474
 475                 fb->fb.screen_base = ioremap_wc(fbinfo.base,
 476                                                 fb->fb.screen_size);
 477         } else {
 478                 fb->fb.screen_base = fb->cpuaddr;
 479         }
 480
 481         if (!fb->fb.screen_base) {
 482                 /* the console may currently be locked */
 483                 console_trylock();
 484                 console_unlock();
 485                 dev_err(info->device, "Failed to set screen_base\n");
 486                 return -ENOMEM;
 487         }
 488
 489         print_debug("%s: start = %p,%p width=%d, height=%d, bpp=%d, pitch=%d size=%d\n",
 490                     __func__, (void *)fb->fb.screen_base,
 491                     (void *)fb->fb_bus_address, fbinfo.xres, fbinfo.yres,
 492                     fbinfo.bpp, fbinfo.pitch, (int)fb->fb.screen_size);
 493
 494         return 0;
 495 }
 496
 497 static inline u32 convert_bitfield(int val, struct fb_bitfield *bf)
 498 {
 499         unsigned int mask = (1 << bf->length) - 1;
 500
 501         return (val >> (16 - bf->length) & mask) << bf->offset;
 502 }
 503
 504 static int bcm2708_fb_setcolreg(unsigned int regno, unsigned int red,
 505                                 unsigned int green, unsigned int blue,
 506                                 unsigned int transp, struct fb_info *info)
 507 {
 508         struct bcm2708_fb *fb = to_bcm2708(info);
 509
 510         if (fb->fb.var.bits_per_pixel <= 8) {
 511                 if (regno < 256) {
 512                         /* blue [23:16], green [15:8], red [7:0] */
 513                         fb->gpu_cmap[regno] = ((red   >> 8) & 0xff) << 0 |
 514                                               ((green >> 8) & 0xff) << 8 |
 515                                               ((blue  >> 8) & 0xff) << 16;
 516                 }
 517                 /* Hack: we need to tell GPU the palette has changed, but
 518                  * currently bcm2708_fb_set_par takes noticeable time when
 519                  * called for every (256) colour
 520                  * So just call it for what looks like the last colour in a
 521                  * list for now.
 522                  */
 523                 if (regno == 15 || regno == 255) {
 524                         struct packet {
 525                                 u32 offset;
 526                                 u32 length;
 527                                 u32 cmap[256];
 528                         } *packet;
 529                         int ret;
 530
 531                         packet = kmalloc(sizeof(*packet), GFP_KERNEL);
 532                         if (!packet)
 533                                 return -ENOMEM;
 534                         packet->offset = 0;
 535                         packet->length = regno + 1;
 536                         memcpy(packet->cmap, fb->gpu_cmap,
 537                                sizeof(packet->cmap));
 538
 539                         set_display_num(fb);
 540
 541                         ret = rpi_firmware_property(fb->fbdev->fw,
 542                                                     RPI_FIRMWARE_FRAMEBUFFER_SET_PALETTE,
 543                                                     packet,
 544                                                     (2 + packet->length) * sizeof(u32));
 545                         if (ret || packet->offset)
 546                                 dev_err(info->device,
 547                                         "Failed to set palette (%d,%u)\n",
 548                                         ret, packet->offset);
 549                         kfree(packet);
 550                 }
 551         } else if (regno < 16) {
 552                 fb->cmap[regno] = convert_bitfield(transp, &fb->fb.var.transp) |
 553                                   convert_bitfield(blue, &fb->fb.var.blue) |
 554                                   convert_bitfield(green, &fb->fb.var.green) |
 555                                   convert_bitfield(red, &fb->fb.var.red);
 556         }
 557         return regno > 255;
 558 }
 559
 560 static int bcm2708_fb_blank(int blank_mode, struct fb_info *info)
 561 {
 562         struct bcm2708_fb *fb = to_bcm2708(info);
 563         u32 value;
 564         int ret;
 565
 566         switch (blank_mode) {
 567         case FB_BLANK_UNBLANK:
 568                 value = 0;
 569                 break;
 570         case FB_BLANK_NORMAL:
 571         case FB_BLANK_VSYNC_SUSPEND:
 572         case FB_BLANK_HSYNC_SUSPEND:
 573         case FB_BLANK_POWERDOWN:
 574                 value = 1;
 575                 break;
 576         default:
 577                 return -EINVAL;
 578         }
 579
 580         set_display_num(fb);
 581
 582         ret = rpi_firmware_property(fb->fbdev->fw, RPI_FIRMWARE_FRAMEBUFFER_BLANK,
 583                                     &value, sizeof(value));
 584
 585         if (ret)
 586                 dev_err(info->device, "%s(%d) failed: %d\n", __func__,
 587                         blank_mode, ret);
 588
 589         return ret;
 590 }
 591
 592 static int bcm2708_fb_pan_display(struct fb_var_screeninfo *var,
 593                                   struct fb_info *info)
 594 {
 595         s32 result;
 596
 597         info->var.xoffset = var->xoffset;
 598         info->var.yoffset = var->yoffset;
 599         result = bcm2708_fb_set_par(info);
 600         if (result != 0)
 601                 pr_err("%s(%u,%u) returns=%d\n", __func__, var->xoffset,
 602                        var->yoffset, result);
 603         return result;
 604 }
 605
 606 static void dma_memcpy(struct bcm2708_fb *fb, dma_addr_t dst, dma_addr_t src,
 607                        int size)
 608 {
 609         struct bcm2708_fb_dev *fbdev = fb->fbdev;
 610         struct bcm2708_dma_cb *cb = fbdev->cb_base;
 611         int burst_size = (fbdev->dma_chan == 0) ? 8 : 2;
 612
 613         cb->info = BCM2708_DMA_BURST(burst_size) | BCM2708_DMA_S_WIDTH |
 614                    BCM2708_DMA_S_INC | BCM2708_DMA_D_WIDTH |
 615                    BCM2708_DMA_D_INC;
 616         cb->dst = dst;
 617         cb->src = src;
 618         cb->length = size;
 619         cb->stride = 0;
 620         cb->pad[0] = 0;
 621         cb->pad[1] = 0;
 622         cb->next = 0;
 623
 624         // Not sure what to do if this gets a signal whilst waiting
 625         if (mutex_lock_interruptible(&fbdev->dma_mutex))
 626                 return;
 627
 628         if (size < dma_busy_wait_threshold) {
 629                 bcm_dma_start(fbdev->dma_chan_base, fbdev->cb_handle);
 630                 bcm_dma_wait_idle(fbdev->dma_chan_base);
 631         } else {
 632                 void __iomem *local_dma_chan = fbdev->dma_chan_base;
 633
 634                 cb->info |= BCM2708_DMA_INT_EN;
 635                 bcm_dma_start(fbdev->dma_chan_base, fbdev->cb_handle);
 636                 while (bcm_dma_is_busy(local_dma_chan)) {
 637                         wait_event_interruptible(fbdev->dma_waitq,
 638                                                  !bcm_dma_is_busy(local_dma_chan));
 639                 }
 640                 fbdev->dma_stats.dma_irqs++;
 641         }
 642         fbdev->dma_stats.dma_copies++;
 643
 644         mutex_unlock(&fbdev->dma_mutex);
 645 }
 646
 647 /* address with no aliases */
 648 #define INTALIAS_NORMAL(x) ((x) & ~0xc0000000)
 649 /* cache coherent but non-allocating in L1 and L2 */
 650 #define INTALIAS_L1L2_NONALLOCATING(x) (((x) & ~0xc0000000) | 0x80000000)
 651
 652 static long vc_mem_copy(struct bcm2708_fb *fb, struct fb_dmacopy *ioparam)
 653 {
 654         size_t size = PAGE_SIZE;
 655         u32 *buf = NULL;
 656         dma_addr_t bus_addr;
 657         long rc = 0;
 658         size_t offset;
 659
 660         /* restrict this to root user */
 661         if (!uid_eq(current_euid(), GLOBAL_ROOT_UID)) {
 662                 rc = -EFAULT;
 663                 goto out;
 664         }
 665
 666         if (!fb->gpu.base || !fb->gpu.length) {
 667                 pr_err("[%s]: Unable to determine gpu memory (%x,%x)\n",
 668                        __func__, fb->gpu.base, fb->gpu.length);
 669                 return -EFAULT;
 670         }
 671
 672         if (INTALIAS_NORMAL(ioparam->src) < fb->gpu.base ||
 673             INTALIAS_NORMAL(ioparam->src) >= fb->gpu.base + fb->gpu.length) {
 674                 pr_err("[%s]: Invalid memory access %x (%x-%x)", __func__,
 675                        INTALIAS_NORMAL(ioparam->src), fb->gpu.base,
 676                        fb->gpu.base + fb->gpu.length);
 677                 return -EFAULT;
 678         }
 679
 680         buf = dma_alloc_coherent(fb->fb.device, PAGE_ALIGN(size), &bus_addr,
 681                                  GFP_ATOMIC);
 682         if (!buf) {
 683                 pr_err("[%s]: failed to dma_alloc_coherent(%zd)\n", __func__,
 684                        size);
 685                 rc = -ENOMEM;
 686                 goto out;
 687         }
 688
 689         for (offset = 0; offset < ioparam->length; offset += size) {
 690                 size_t remaining = ioparam->length - offset;
 691                 size_t s = min(size, remaining);
 692                 u8 *p = (u8 *)((uintptr_t)ioparam->src + offset);
 693                 u8 *q = (u8 *)ioparam->dst + offset;
 694
 695                 dma_memcpy(fb, bus_addr,
 696                            INTALIAS_L1L2_NONALLOCATING((u32)(uintptr_t)p),
 697                                                        size);
 698                 if (copy_to_user(q, buf, s) != 0) {
 699                         pr_err("[%s]: failed to copy-to-user\n", __func__);
 700                         rc = -EFAULT;
 701                         goto out;
 702                 }
 703         }
 704 out:
 705         if (buf)
 706                 dma_free_coherent(fb->fb.device, PAGE_ALIGN(size), buf,
 707                                   bus_addr);
 708         return rc;
 709 }
 710
 711 static int bcm2708_ioctl(struct fb_info *info, unsigned int cmd,
 712                          unsigned long arg)
 713 {
 714         struct bcm2708_fb *fb = to_bcm2708(info);
 715         u32 dummy = 0;
 716         int ret;
 717
 718         switch (cmd) {
 719         case FBIO_WAITFORVSYNC:
 720                 set_display_num(fb);
 721
 722                 ret = rpi_firmware_property(fb->fbdev->fw,
 723                                             RPI_FIRMWARE_FRAMEBUFFER_SET_VSYNC,
 724                                             &dummy, sizeof(dummy));
 725                 break;
 726
 727         case FBIODMACOPY:
 728         {
 729                 struct fb_dmacopy ioparam;
 730                 /* Get the parameter data.
 731                  */
 732                 if (copy_from_user
 733                     (&ioparam, (void *)arg, sizeof(ioparam))) {
 734                         pr_err("[%s]: failed to copy-from-user\n", __func__);
 735                         ret = -EFAULT;
 736                         break;
 737                 }
 738                 ret = vc_mem_copy(fb, &ioparam);
 739                 break;
 740         }
 741         default:
 742                 dev_dbg(info->device, "Unknown ioctl 0x%x\n", cmd);
 743                 return -ENOTTY;
 744         }
 745
 746         if (ret)
 747                 dev_err(info->device, "ioctl 0x%x failed (%d)\n", cmd, ret);
 748
 749         return ret;
 750 }
 751
 752 #ifdef CONFIG_COMPAT
 753 struct fb_dmacopy32 {
 754         compat_uptr_t dst;
 755         __u32 src;
 756         __u32 length;
 757 };
 758
 759 #define FBIODMACOPY32           _IOW('z', 0x22, struct fb_dmacopy32)
 760
 761 static int bcm2708_compat_ioctl(struct fb_info *info, unsigned int cmd,
 762                                 unsigned long arg)
 763 {
 764         struct bcm2708_fb *fb = to_bcm2708(info);
 765         int ret;
 766
 767         switch (cmd) {
 768         case FBIODMACOPY32:
 769         {
 770                 struct fb_dmacopy32 param32;
 771                 struct fb_dmacopy param;
 772                 /* Get the parameter data.
 773                  */
 774                 if (copy_from_user(&param32, (void *)arg, sizeof(param32))) {
 775                         pr_err("[%s]: failed to copy-from-user\n", __func__);
 776                         ret = -EFAULT;
 777                         break;
 778                 }
 779                 param.dst = compat_ptr(param32.dst);
 780                 param.src = param32.src;
 781                 param.length = param32.length;
 782                 ret = vc_mem_copy(fb, &param);
 783                 break;
 784         }
 785         default:
 786                 ret = bcm2708_ioctl(info, cmd, arg);
 787                 break;
 788         }
 789         return ret;
 790 }
 791 #endif
 792
 793 static void bcm2708_fb_fillrect(struct fb_info *info,
 794                                 const struct fb_fillrect *rect)
 795 {
 796         cfb_fillrect(info, rect);
 797 }
 798
 799 /* A helper function for configuring dma control block */
 800 static void set_dma_cb(struct bcm2708_dma_cb *cb,
 801                 int        burst_size,
 802                 dma_addr_t dst,
 803                 int        dst_stride,
 804                 dma_addr_t src,
 805                 int        src_stride,
 806                 int        w,
 807                 int        h)
 808 {
 809         cb->info = BCM2708_DMA_BURST(burst_size) | BCM2708_DMA_S_WIDTH |
 810                 BCM2708_DMA_S_INC | BCM2708_DMA_D_WIDTH |
 811                 BCM2708_DMA_D_INC | BCM2708_DMA_TDMODE;
 812         cb->dst = dst;
 813         cb->src = src;
 814         /*
 815          * This is not really obvious from the DMA documentation,
 816          * but the top 16 bits must be programmmed to "height -1"
 817          * and not "height" in 2D mode.
 818          */
 819         cb->length = ((h - 1) << 16) | w;
 820         cb->stride = ((dst_stride - w) << 16) | (u16)(src_stride - w);
 821         cb->pad[0] = 0;
 822         cb->pad[1] = 0;
 823 }
 824
 825 static void bcm2708_fb_copyarea(struct fb_info *info,
 826                                 const struct fb_copyarea *region)
 827 {
 828         struct bcm2708_fb *fb = to_bcm2708(info);
 829         struct bcm2708_fb_dev *fbdev = fb->fbdev;
 830         struct bcm2708_dma_cb *cb = fbdev->cb_base;
 831         int bytes_per_pixel = (info->var.bits_per_pixel + 7) >> 3;
 832
 833         /* Channel 0 supports larger bursts and is a bit faster */
 834         int burst_size = (fbdev->dma_chan == 0) ? 8 : 2;
 835         int pixels = region->width * region->height;
 836
 837         /* If DMA is currently in use (ie being used on another FB), then
 838          * rather than wait for it to finish, just use the cfb_copyarea
 839          */
 840         if (!mutex_trylock(&fbdev->dma_mutex) ||
 841             bytes_per_pixel > 4 ||
 842             info->var.xres * info->var.yres > 1920 * 1200 ||
 843             region->width <= 0 || region->width > info->var.xres ||
 844             region->height <= 0 || region->height > info->var.yres ||
 845             region->sx < 0 || region->sx >= info->var.xres ||
 846             region->sy < 0 || region->sy >= info->var.yres ||
 847             region->dx < 0 || region->dx >= info->var.xres ||
 848             region->dy < 0 || region->dy >= info->var.yres ||
 849             region->sx + region->width > info->var.xres ||
 850             region->dx + region->width > info->var.xres ||
 851             region->sy + region->height > info->var.yres ||
 852             region->dy + region->height > info->var.yres) {
 853                 cfb_copyarea(info, region);
 854                 return;
 855         }
 856
 857         if (region->dy == region->sy && region->dx > region->sx) {
 858                 /*
 859                  * A difficult case of overlapped copy. Because DMA can't
 860                  * copy individual scanlines in backwards direction, we need
 861                  * two-pass processing. We do it by programming a chain of dma
 862                  * control blocks in the first 16K part of the buffer and use
 863                  * the remaining 48K as the intermediate temporary scratch
 864                  * buffer. The buffer size is sufficient to handle up to
 865                  * 1920x1200 resolution at 32bpp pixel depth.
 866                  */
 867                 int y;
 868                 dma_addr_t control_block_pa = fbdev->cb_handle;
 869                 dma_addr_t scratchbuf = fbdev->cb_handle + 16 * 1024;
 870                 int scanline_size = bytes_per_pixel * region->width;
 871                 int scanlines_per_cb = (64 * 1024 - 16 * 1024) / scanline_size;
 872
 873                 for (y = 0; y < region->height; y += scanlines_per_cb) {
 874                         dma_addr_t src =
 875                                 fb->fb_bus_address +
 876                                 bytes_per_pixel * region->sx +
 877                                 (region->sy + y) * fb->fb.fix.line_length;
 878                         dma_addr_t dst =
 879                                 fb->fb_bus_address +
 880                                 bytes_per_pixel * region->dx +
 881                                 (region->dy + y) * fb->fb.fix.line_length;
 882
 883                         if (region->height - y < scanlines_per_cb)
 884                                 scanlines_per_cb = region->height - y;
 885
 886                         set_dma_cb(cb, burst_size, scratchbuf, scanline_size,
 887                                    src, fb->fb.fix.line_length,
 888                                    scanline_size, scanlines_per_cb);
 889                         control_block_pa += sizeof(struct bcm2708_dma_cb);
 890                         cb->next = control_block_pa;
 891                         cb++;
 892
 893                         set_dma_cb(cb, burst_size, dst, fb->fb.fix.line_length,
 894                                    scratchbuf, scanline_size,
 895                                    scanline_size, scanlines_per_cb);
 896                         control_block_pa += sizeof(struct bcm2708_dma_cb);
 897                         cb->next = control_block_pa;
 898                         cb++;
 899                 }
 900                 /* move the pointer back to the last dma control block */
 901                 cb--;
 902         } else {
 903                 /* A single dma control block is enough. */
 904                 int sy, dy, stride;
 905
 906                 if (region->dy <= region->sy) {
 907                         /* processing from top to bottom */
 908                         dy = region->dy;
 909                         sy = region->sy;
 910                         stride = fb->fb.fix.line_length;
 911                 } else {
 912                         /* processing from bottom to top */
 913                         dy = region->dy + region->height - 1;
 914                         sy = region->sy + region->height - 1;
 915                         stride = -fb->fb.fix.line_length;
 916                 }
 917                 set_dma_cb(cb, burst_size,
 918                            fb->fb_bus_address + dy * fb->fb.fix.line_length +
 919                            bytes_per_pixel * region->dx,
 920                            stride,
 921                            fb->fb_bus_address + sy * fb->fb.fix.line_length +
 922                            bytes_per_pixel * region->sx,
 923                            stride,
 924                            region->width * bytes_per_pixel,
 925                            region->height);
 926         }
 927
 928         /* end of dma control blocks chain */
 929         cb->next = 0;
 930
 931         if (pixels < dma_busy_wait_threshold) {
 932                 bcm_dma_start(fbdev->dma_chan_base, fbdev->cb_handle);
 933                 bcm_dma_wait_idle(fbdev->dma_chan_base);
 934         } else {
 935                 void __iomem *local_dma_chan = fbdev->dma_chan_base;
 936
 937                 cb->info |= BCM2708_DMA_INT_EN;
 938                 bcm_dma_start(fbdev->dma_chan_base, fbdev->cb_handle);
 939                 while (bcm_dma_is_busy(local_dma_chan)) {
 940                         wait_event_interruptible(fbdev->dma_waitq,
 941                                                  !bcm_dma_is_busy(local_dma_chan));
 942                 }
 943                 fbdev->dma_stats.dma_irqs++;
 944         }
 945         fbdev->dma_stats.dma_copies++;
 946
 947         mutex_unlock(&fbdev->dma_mutex);
 948 }
 949
 950 static void bcm2708_fb_imageblit(struct fb_info *info,
 951                                  const struct fb_image *image)
 952 {
 953         cfb_imageblit(info, image);
 954 }
 955
 956 static irqreturn_t bcm2708_fb_dma_irq(int irq, void *cxt)
 957 {
 958         struct bcm2708_fb_dev *fbdev = cxt;
 959
 960         /* FIXME: should read status register to check if this is
 961          * actually interrupting us or not, in case this interrupt
 962          * ever becomes shared amongst several DMA channels
 963          *
 964          * readl(dma_chan_base + BCM2708_DMA_CS) & BCM2708_DMA_IRQ;
 965          */
 966
 967         /* acknowledge the interrupt */
 968         writel(BCM2708_DMA_INT, fbdev->dma_chan_base + BCM2708_DMA_CS);
 969
 970         wake_up(&fbdev->dma_waitq);
 971         return IRQ_HANDLED;
 972 }
 973
 974 static struct fb_ops bcm2708_fb_ops = {
 975         .owner = THIS_MODULE,
 976         .fb_check_var = bcm2708_fb_check_var,
 977         .fb_set_par = bcm2708_fb_set_par,
 978         .fb_setcolreg = bcm2708_fb_setcolreg,
 979         .fb_blank = bcm2708_fb_blank,
 980         .fb_fillrect = bcm2708_fb_fillrect,
 981         .fb_copyarea = bcm2708_fb_copyarea,
 982         .fb_imageblit = bcm2708_fb_imageblit,
 983         .fb_pan_display = bcm2708_fb_pan_display,
 984         .fb_ioctl = bcm2708_ioctl,
 985 #ifdef CONFIG_COMPAT
 986         .fb_compat_ioctl = bcm2708_compat_ioctl,
 987 #endif
 988 };
 989
 990 static int bcm2708_fb_register(struct bcm2708_fb *fb)
 991 {
 992         int ret;
 993
 994         fb->fb.fbops = &bcm2708_fb_ops;
 995         fb->fb.flags = FBINFO_FLAG_DEFAULT | FBINFO_HWACCEL_COPYAREA;
 996         fb->fb.pseudo_palette = fb->cmap;
 997
 998         strncpy(fb->fb.fix.id, bcm2708_name, sizeof(fb->fb.fix.id));
 999         fb->fb.fix.type = FB_TYPE_PACKED_PIXELS;
1000         fb->fb.fix.type_aux = 0;
1001         fb->fb.fix.xpanstep = 1;
1002         fb->fb.fix.ypanstep = 1;
1003         fb->fb.fix.ywrapstep = 0;
1004         fb->fb.fix.accel = FB_ACCEL_NONE;
1005
1006         /* If we have data from the VC4 on FB's, use that, otherwise use the
1007          * module parameters
1008          */
1009         if (fb->display_settings.width) {
1010                 fb->fb.var.xres = fb->display_settings.width;
1011                 fb->fb.var.yres = fb->display_settings.height;
1012                 fb->fb.var.xres_virtual = fb->fb.var.xres;
1013                 fb->fb.var.yres_virtual = fb->fb.var.yres;
1014                 fb->fb.var.bits_per_pixel = fb->display_settings.depth;
1015         } else {
1016                 fb->fb.var.xres = fbwidth;
1017                 fb->fb.var.yres = fbheight;
1018                 fb->fb.var.xres_virtual = fbwidth;
1019                 fb->fb.var.yres_virtual = fbheight;
1020                 fb->fb.var.bits_per_pixel = fbdepth;
1021         }
1022
1023         fb->fb.var.vmode = FB_VMODE_NONINTERLACED;
1024         fb->fb.var.activate = FB_ACTIVATE_NOW;
1025         fb->fb.var.nonstd = 0;
1026         fb->fb.var.height = -1;         /* height of picture in mm    */
1027         fb->fb.var.width = -1;          /* width of picture in mm    */
1028         fb->fb.var.accel_flags = 0;
1029
1030         fb->fb.monspecs.hfmin = 0;
1031         fb->fb.monspecs.hfmax = 100000;
1032         fb->fb.monspecs.vfmin = 0;
1033         fb->fb.monspecs.vfmax = 400;
1034         fb->fb.monspecs.dclkmin = 1000000;
1035         fb->fb.monspecs.dclkmax = 100000000;
1036
1037         bcm2708_fb_set_bitfields(&fb->fb.var);
1038
1039         /*
1040          * Allocate colourmap.
1041          */
1042         fb_set_var(&fb->fb, &fb->fb.var);
1043
1044         ret = bcm2708_fb_set_par(&fb->fb);
1045
1046         if (ret)
1047                 return ret;
1048
1049         ret = register_framebuffer(&fb->fb);
1050
1051         if (ret == 0)
1052                 goto out;
1053
1054         dev_warn(fb->fb.dev, "Unable to register framebuffer (%d)\n", ret);
1055 out:
1056         return ret;
1057 }
1058
1059 static int bcm2708_fb_probe(struct platform_device *dev)
1060 {
1061         struct device_node *fw_np;
1062         struct rpi_firmware *fw;
1063         int ret, i;
1064         u32 num_displays;
1065         struct bcm2708_fb_dev *fbdev;
1066         struct { u32 base, length; } gpu_mem;
1067
1068         fbdev = devm_kzalloc(&dev->dev, sizeof(*fbdev), GFP_KERNEL);
1069
1070         if (!fbdev)
1071                 return -ENOMEM;
1072
1073         fw_np = of_parse_phandle(dev->dev.of_node, "firmware", 0);
1074
1075 /* Remove comment when booting without Device Tree is no longer supported
1076  *      if (!fw_np) {
1077  *              dev_err(&dev->dev, "Missing firmware node\n");
1078  *              return -ENOENT;
1079  *      }
1080  */
1081         fw = rpi_firmware_get(fw_np);
1082         fbdev->fw = fw;
1083
1084         if (!fw)
1085                 return -EPROBE_DEFER;
1086
1087         ret = rpi_firmware_property(fw,
1088                                     RPI_FIRMWARE_FRAMEBUFFER_GET_NUM_DISPLAYS,
1089                                     &num_displays, sizeof(u32));
1090
1091         /* If we fail to get the number of displays, or it returns 0, then
1092          * assume old firmware that doesn't have the mailbox call, so just
1093          * set one display
1094          */
1095         if (ret || num_displays == 0) {
1096                 dev_err(&dev->dev,
1097                         "Unable to determine number of FBs. Disabling driver.\n");
1098                 return -ENOENT;
1099         } else {
1100                 fbdev->firmware_supports_multifb = 1;
1101         }
1102
1103         if (num_displays > MAX_FRAMEBUFFERS) {
1104                 dev_warn(&dev->dev,
1105                          "More displays reported from firmware than supported in driver (%u vs %u)",
1106                          num_displays, MAX_FRAMEBUFFERS);
1107                 num_displays = MAX_FRAMEBUFFERS;
1108         }
1109
1110         dev_info(&dev->dev, "FB found %d display(s)\n", num_displays);
1111
1112         /* Set up the DMA information. Note we have just one set of DMA
1113          * parameters to work with all the FB's so requires synchronising when
1114          * being used
1115          */
1116
1117         mutex_init(&fbdev->dma_mutex);
1118
1119         fbdev->cb_base = dma_alloc_wc(&dev->dev, SZ_64K,
1120                                                 &fbdev->cb_handle,
1121                                                 GFP_KERNEL);
1122         if (!fbdev->cb_base) {
1123                 dev_err(&dev->dev, "cannot allocate DMA CBs\n");
1124                 ret = -ENOMEM;
1125                 goto free_fb;
1126         }
1127
1128         ret = bcm_dma_chan_alloc(BCM_DMA_FEATURE_BULK,
1129                                  &fbdev->dma_chan_base,
1130                                  &fbdev->dma_irq);
1131         if (ret < 0) {
1132                 dev_err(&dev->dev, "Couldn't allocate a DMA channel\n");
1133                 goto free_cb;
1134         }
1135         fbdev->dma_chan = ret;
1136
1137         ret = request_irq(fbdev->dma_irq, bcm2708_fb_dma_irq,
1138                           0, "bcm2708_fb DMA", fbdev);
1139         if (ret) {
1140                 dev_err(&dev->dev,
1141                         "Failed to request DMA irq\n");
1142                 goto free_dma_chan;
1143         }
1144
1145         rpi_firmware_property(fbdev->fw,
1146                               RPI_FIRMWARE_GET_VC_MEMORY,
1147                               &gpu_mem, sizeof(gpu_mem));
1148
1149         for (i = 0; i < num_displays; i++) {
1150                 struct bcm2708_fb *fb = &fbdev->displays[i];
1151
1152                 fb->display_settings.display_num = i;
1153                 fb->dev = dev;
1154                 fb->fb.device = &dev->dev;
1155                 fb->fbdev = fbdev;
1156
1157                 fb->gpu.base = gpu_mem.base;
1158                 fb->gpu.length = gpu_mem.length;
1159
1160                 if (fbdev->firmware_supports_multifb) {
1161                         ret = rpi_firmware_property(fw,
1162                                                     RPI_FIRMWARE_FRAMEBUFFER_GET_DISPLAY_SETTINGS,
1163                                                     &fb->display_settings,
1164                                                     GET_DISPLAY_SETTINGS_PAYLOAD_SIZE);
1165                 } else {
1166                         memset(&fb->display_settings, 0,
1167                                sizeof(fb->display_settings));
1168                 }
1169
1170                 ret = bcm2708_fb_register(fb);
1171
1172                 if (ret == 0) {
1173                         bcm2708_fb_debugfs_init(fb);
1174
1175                         fbdev->num_displays++;
1176
1177                         dev_info(&dev->dev,
1178                                  "Registered framebuffer for display %u, size %ux%u\n",
1179                                  fb->display_settings.display_num,
1180                                  fb->fb.var.xres,
1181                                  fb->fb.var.yres);
1182                 } else {
1183                         // Use this to flag if this FB entry is in use.
1184                         fb->fbdev = NULL;
1185                 }
1186         }
1187
1188         // Did we actually successfully create any FB's?
1189         if (fbdev->num_displays) {
1190                 init_waitqueue_head(&fbdev->dma_waitq);
1191                 platform_set_drvdata(dev, fbdev);
1192                 return ret;
1193         }
1194
1195 free_dma_chan:
1196         bcm_dma_chan_free(fbdev->dma_chan);
1197 free_cb:
1198         dma_free_wc(&dev->dev, SZ_64K, fbdev->cb_base,
1199                               fbdev->cb_handle);
1200 free_fb:
1201         dev_err(&dev->dev, "probe failed, err %d\n", ret);
1202
1203         return ret;
1204 }
1205
1206 static int bcm2708_fb_remove(struct platform_device *dev)
1207 {
1208         struct bcm2708_fb_dev *fbdev = platform_get_drvdata(dev);
1209         int i;
1210
1211         platform_set_drvdata(dev, NULL);
1212
1213         for (i = 0; i < fbdev->num_displays; i++) {
1214                 if (fbdev->displays[i].fb.screen_base)
1215                         iounmap(fbdev->displays[i].fb.screen_base);
1216
1217                 if (fbdev->displays[i].fbdev) {
1218                         unregister_framebuffer(&fbdev->displays[i].fb);
1219                         bcm2708_fb_debugfs_deinit(&fbdev->displays[i]);
1220                 }
1221         }
1222
1223         dma_free_wc(&dev->dev, SZ_64K, fbdev->cb_base,
1224                               fbdev->cb_handle);
1225         bcm_dma_chan_free(fbdev->dma_chan);
1226         free_irq(fbdev->dma_irq, fbdev);
1227
1228         mutex_destroy(&fbdev->dma_mutex);
1229
1230         return 0;
1231 }
1232
1233 static const struct of_device_id bcm2708_fb_of_match_table[] = {
1234         { .compatible = "brcm,bcm2708-fb", },
1235         {},
1236 };
1237 MODULE_DEVICE_TABLE(of, bcm2708_fb_of_match_table);
1238
1239 static struct platform_driver bcm2708_fb_driver = {
1240         .probe = bcm2708_fb_probe,
1241         .remove = bcm2708_fb_remove,
1242         .driver = {
1243                   .name = DRIVER_NAME,
1244                   .owner = THIS_MODULE,
1245                   .of_match_table = bcm2708_fb_of_match_table,
1246                   },
1247 };
1248
1249 static int __init bcm2708_fb_init(void)
1250 {
1251         return platform_driver_register(&bcm2708_fb_driver);
1252 }
1253
1254 module_init(bcm2708_fb_init);
1255
1256 static void __exit bcm2708_fb_exit(void)
1257 {
1258         platform_driver_unregister(&bcm2708_fb_driver);
1259 }
1260
1261 module_exit(bcm2708_fb_exit);
1262
1263 module_param(fbwidth, int, 0644);
1264 module_param(fbheight, int, 0644);
1265 module_param(fbdepth, int, 0644);
1266 module_param(fbswap, int, 0644);
1267
1268 MODULE_DESCRIPTION("BCM2708 framebuffer driver");
1269 MODULE_LICENSE("GPL");
1270
1271 MODULE_PARM_DESC(fbwidth, "Width of ARM Framebuffer");
1272 MODULE_PARM_DESC(fbheight, "Height of ARM Framebuffer");
1273 MODULE_PARM_DESC(fbdepth, "Bit depth of ARM Framebuffer");
1274 MODULE_PARM_DESC(fbswap, "Swap order of red and blue in 24 and 32 bit modes");