drivers/video/fbdev/metronomefb.c

   1 /*
   2  * linux/drivers/video/metronomefb.c -- FB driver for Metronome controller
   3  *
   4  * Copyright (C) 2008, Jaya Kumar
   5  *
   6  * This file is subject to the terms and conditions of the GNU General Public
   7  * License. See the file COPYING in the main directory of this archive for
   8  * more details.
   9  *
  10  * Layout is based on skeletonfb.c by James Simmons and Geert Uytterhoeven.
  11  *
  12  * This work was made possible by help and equipment support from E-Ink
  13  * Corporation. https://www.eink.com/
  14  *
  15  * This driver is written to be used with the Metronome display controller.
  16  * It is intended to be architecture independent. A board specific driver
  17  * must be used to perform all the physical IO interactions. An example
  18  * is provided as am200epd.c
  19  *
  20  */
  21 #include <linux/module.h>
  22 #include <linux/kernel.h>
  23 #include <linux/errno.h>
  24 #include <linux/string.h>
  25 #include <linux/mm.h>
  26 #include <linux/vmalloc.h>
  27 #include <linux/delay.h>
  28 #include <linux/interrupt.h>
  29 #include <linux/fb.h>
  30 #include <linux/init.h>
  31 #include <linux/platform_device.h>
  32 #include <linux/list.h>
  33 #include <linux/firmware.h>
  34 #include <linux/dma-mapping.h>
  35 #include <linux/uaccess.h>
  36 #include <linux/irq.h>
  37
  38 #include <video/metronomefb.h>
  39
  40 #include <asm/unaligned.h>
  41
  42 /* Display specific information */
  43 #define DPY_W 832
  44 #define DPY_H 622
  45
  46 static int user_wfm_size;
  47
  48 /* frame differs from image. frame includes non-visible pixels */
  49 struct epd_frame {
  50         int fw; /* frame width */
  51         int fh; /* frame height */
  52         u16 config[4];
  53         int wfm_size;
  54 };
  55
  56 static struct epd_frame epd_frame_table[] = {
  57         {
  58                 .fw = 832,
  59                 .fh = 622,
  60                 .config = {
  61                         15 /* sdlew */
  62                         | 2 << 8 /* sdosz */
  63                         | 0 << 11 /* sdor */
  64                         | 0 << 12 /* sdces */
  65                         | 0 << 15, /* sdcer */
  66                         42 /* gdspl */
  67                         | 1 << 8 /* gdr1 */
  68                         | 1 << 9 /* sdshr */
  69                         | 0 << 15, /* gdspp */
  70                         18 /* gdspw */
  71                         | 0 << 15, /* dispc */
  72                         599 /* vdlc */
  73                         | 0 << 11 /* dsi */
  74                         | 0 << 12, /* dsic */
  75                 },
  76                 .wfm_size = 47001,
  77         },
  78         {
  79                 .fw = 1088,
  80                 .fh = 791,
  81                 .config = {
  82                         0x0104,
  83                         0x031f,
  84                         0x0088,
  85                         0x02ff,
  86                 },
  87                 .wfm_size = 46770,
  88         },
  89         {
  90                 .fw = 1200,
  91                 .fh = 842,
  92                 .config = {
  93                         0x0101,
  94                         0x030e,
  95                         0x0012,
  96                         0x0280,
  97                 },
  98                 .wfm_size = 46770,
  99         },
 100 };
 101
 102 static struct fb_fix_screeninfo metronomefb_fix = {
 103         .id =           "metronomefb",
 104         .type =         FB_TYPE_PACKED_PIXELS,
 105         .visual =       FB_VISUAL_STATIC_PSEUDOCOLOR,
 106         .xpanstep =     0,
 107         .ypanstep =     0,
 108         .ywrapstep =    0,
 109         .line_length =  DPY_W,
 110         .accel =        FB_ACCEL_NONE,
 111 };
 112
 113 static struct fb_var_screeninfo metronomefb_var = {
 114         .xres           = DPY_W,
 115         .yres           = DPY_H,
 116         .xres_virtual   = DPY_W,
 117         .yres_virtual   = DPY_H,
 118         .bits_per_pixel = 8,
 119         .grayscale      = 1,
 120         .nonstd         = 1,
 121         .red =          { 4, 3, 0 },
 122         .green =        { 0, 0, 0 },
 123         .blue =         { 0, 0, 0 },
 124         .transp =       { 0, 0, 0 },
 125 };
 126
 127 /* the waveform structure that is coming from userspace firmware */
 128 struct waveform_hdr {
 129         u8 stuff[32];
 130
 131         u8 wmta[3];
 132         u8 fvsn;
 133
 134         u8 luts;
 135         u8 mc;
 136         u8 trc;
 137         u8 stuff3;
 138
 139         u8 endb;
 140         u8 swtb;
 141         u8 stuff2a[2];
 142
 143         u8 stuff2b[3];
 144         u8 wfm_cs;
 145 } __attribute__ ((packed));
 146
 147 /* main metronomefb functions */
 148 static u8 calc_cksum(int start, int end, u8 *mem)
 149 {
 150         u8 tmp = 0;
 151         int i;
 152
 153         for (i = start; i < end; i++)
 154                 tmp += mem[i];
 155
 156         return tmp;
 157 }
 158
 159 static u16 calc_img_cksum(u16 *start, int length)
 160 {
 161         u16 tmp = 0;
 162
 163         while (length--)
 164                 tmp += *start++;
 165
 166         return tmp;
 167 }
 168
 169 /* here we decode the incoming waveform file and populate metromem */
 170 static int load_waveform(u8 *mem, size_t size, int m, int t,
 171                          struct metronomefb_par *par)
 172 {
 173         int tta;
 174         int wmta;
 175         int trn = 0;
 176         int i;
 177         unsigned char v;
 178         u8 cksum;
 179         int cksum_idx;
 180         int wfm_idx, owfm_idx;
 181         int mem_idx = 0;
 182         struct waveform_hdr *wfm_hdr;
 183         u8 *metromem = par->metromem_wfm;
 184         struct device *dev = par->info->dev;
 185
 186         if (user_wfm_size)
 187                 epd_frame_table[par->dt].wfm_size = user_wfm_size;
 188
 189         if (size != epd_frame_table[par->dt].wfm_size) {
 190                 dev_err(dev, "Error: unexpected size %zd != %d\n", size,
 191                                         epd_frame_table[par->dt].wfm_size);
 192                 return -EINVAL;
 193         }
 194
 195         wfm_hdr = (struct waveform_hdr *) mem;
 196
 197         if (wfm_hdr->fvsn != 1) {
 198                 dev_err(dev, "Error: bad fvsn %x\n", wfm_hdr->fvsn);
 199                 return -EINVAL;
 200         }
 201         if (wfm_hdr->luts != 0) {
 202                 dev_err(dev, "Error: bad luts %x\n", wfm_hdr->luts);
 203                 return -EINVAL;
 204         }
 205         cksum = calc_cksum(32, 47, mem);
 206         if (cksum != wfm_hdr->wfm_cs) {
 207                 dev_err(dev, "Error: bad cksum %x != %x\n", cksum,
 208                                         wfm_hdr->wfm_cs);
 209                 return -EINVAL;
 210         }
 211         wfm_hdr->mc += 1;
 212         wfm_hdr->trc += 1;
 213         for (i = 0; i < 5; i++) {
 214                 if (*(wfm_hdr->stuff2a + i) != 0) {
 215                         dev_err(dev, "Error: unexpected value in padding\n");
 216                         return -EINVAL;
 217                 }
 218         }
 219
 220         /* calculating trn. trn is something used to index into
 221         the waveform. presumably selecting the right one for the
 222         desired temperature. it works out the offset of the first
 223         v that exceeds the specified temperature */
 224         if ((sizeof(*wfm_hdr) + wfm_hdr->trc) > size)
 225                 return -EINVAL;
 226
 227         for (i = sizeof(*wfm_hdr); i <= sizeof(*wfm_hdr) + wfm_hdr->trc; i++) {
 228                 if (mem[i] > t) {
 229                         trn = i - sizeof(*wfm_hdr) - 1;
 230                         break;
 231                 }
 232         }
 233
 234         /* check temperature range table checksum */
 235         cksum_idx = sizeof(*wfm_hdr) + wfm_hdr->trc + 1;
 236         if (cksum_idx >= size)
 237                 return -EINVAL;
 238         cksum = calc_cksum(sizeof(*wfm_hdr), cksum_idx, mem);
 239         if (cksum != mem[cksum_idx]) {
 240                 dev_err(dev, "Error: bad temperature range table cksum"
 241                                 " %x != %x\n", cksum, mem[cksum_idx]);
 242                 return -EINVAL;
 243         }
 244
 245         /* check waveform mode table address checksum */
 246         wmta = get_unaligned_le32(wfm_hdr->wmta) & 0x00FFFFFF;
 247         cksum_idx = wmta + m*4 + 3;
 248         if (cksum_idx >= size)
 249                 return -EINVAL;
 250         cksum = calc_cksum(cksum_idx - 3, cksum_idx, mem);
 251         if (cksum != mem[cksum_idx]) {
 252                 dev_err(dev, "Error: bad mode table address cksum"
 253                                 " %x != %x\n", cksum, mem[cksum_idx]);
 254                 return -EINVAL;
 255         }
 256
 257         /* check waveform temperature table address checksum */
 258         tta = get_unaligned_le32(mem + wmta + m * 4) & 0x00FFFFFF;
 259         cksum_idx = tta + trn*4 + 3;
 260         if (cksum_idx >= size)
 261                 return -EINVAL;
 262         cksum = calc_cksum(cksum_idx - 3, cksum_idx, mem);
 263         if (cksum != mem[cksum_idx]) {
 264                 dev_err(dev, "Error: bad temperature table address cksum"
 265                         " %x != %x\n", cksum, mem[cksum_idx]);
 266                 return -EINVAL;
 267         }
 268
 269         /* here we do the real work of putting the waveform into the
 270         metromem buffer. this does runlength decoding of the waveform */
 271         wfm_idx = get_unaligned_le32(mem + tta + trn * 4) & 0x00FFFFFF;
 272         owfm_idx = wfm_idx;
 273         if (wfm_idx >= size)
 274                 return -EINVAL;
 275         while (wfm_idx < size) {
 276                 unsigned char rl;
 277                 v = mem[wfm_idx++];
 278                 if (v == wfm_hdr->swtb) {
 279                         while (((v = mem[wfm_idx++]) != wfm_hdr->swtb) &&
 280                                 wfm_idx < size)
 281                                 metromem[mem_idx++] = v;
 282
 283                         continue;
 284                 }
 285
 286                 if (v == wfm_hdr->endb)
 287                         break;
 288
 289                 rl = mem[wfm_idx++];
 290                 for (i = 0; i <= rl; i++)
 291                         metromem[mem_idx++] = v;
 292         }
 293
 294         cksum_idx = wfm_idx;
 295         if (cksum_idx >= size)
 296                 return -EINVAL;
 297         cksum = calc_cksum(owfm_idx, cksum_idx, mem);
 298         if (cksum != mem[cksum_idx]) {
 299                 dev_err(dev, "Error: bad waveform data cksum"
 300                                 " %x != %x\n", cksum, mem[cksum_idx]);
 301                 return -EINVAL;
 302         }
 303         par->frame_count = (mem_idx/64);
 304
 305         return 0;
 306 }
 307
 308 static int metronome_display_cmd(struct metronomefb_par *par)
 309 {
 310         int i;
 311         u16 cs;
 312         u16 opcode;
 313         static u8 borderval;
 314
 315         /* setup display command
 316         we can't immediately set the opcode since the controller
 317         will try parse the command before we've set it all up
 318         so we just set cs here and set the opcode at the end */
 319
 320         if (par->metromem_cmd->opcode == 0xCC40)
 321                 opcode = cs = 0xCC41;
 322         else
 323                 opcode = cs = 0xCC40;
 324
 325         /* set the args ( 2 bytes ) for display */
 326         i = 0;
 327         par->metromem_cmd->args[i] =    1 << 3 /* border update */
 328                                         | ((borderval++ % 4) & 0x0F) << 4
 329                                         | (par->frame_count - 1) << 8;
 330         cs += par->metromem_cmd->args[i++];
 331
 332         /* the rest are 0 */
 333         memset((u8 *) (par->metromem_cmd->args + i), 0, (32-i)*2);
 334
 335         par->metromem_cmd->csum = cs;
 336         par->metromem_cmd->opcode = opcode; /* display cmd */
 337
 338         return par->board->met_wait_event_intr(par);
 339 }
 340
 341 static int metronome_powerup_cmd(struct metronomefb_par *par)
 342 {
 343         int i;
 344         u16 cs;
 345
 346         /* setup power up command */
 347         par->metromem_cmd->opcode = 0x1234; /* pwr up pseudo cmd */
 348         cs = par->metromem_cmd->opcode;
 349
 350         /* set pwr1,2,3 to 1024 */
 351         for (i = 0; i < 3; i++) {
 352                 par->metromem_cmd->args[i] = 1024;
 353                 cs += par->metromem_cmd->args[i];
 354         }
 355
 356         /* the rest are 0 */
 357         memset(&par->metromem_cmd->args[i], 0,
 358                (ARRAY_SIZE(par->metromem_cmd->args) - i) * 2);
 359
 360         par->metromem_cmd->csum = cs;
 361
 362         msleep(1);
 363         par->board->set_rst(par, 1);
 364
 365         msleep(1);
 366         par->board->set_stdby(par, 1);
 367
 368         return par->board->met_wait_event(par);
 369 }
 370
 371 static int metronome_config_cmd(struct metronomefb_par *par)
 372 {
 373         /* setup config command
 374         we can't immediately set the opcode since the controller
 375         will try parse the command before we've set it all up */
 376
 377         memcpy(par->metromem_cmd->args, epd_frame_table[par->dt].config,
 378                 sizeof(epd_frame_table[par->dt].config));
 379         /* the rest are 0 */
 380         memset(&par->metromem_cmd->args[4], 0,
 381                (ARRAY_SIZE(par->metromem_cmd->args) - 4) * 2);
 382
 383         par->metromem_cmd->csum = 0xCC10;
 384         par->metromem_cmd->csum += calc_img_cksum(par->metromem_cmd->args, 4);
 385         par->metromem_cmd->opcode = 0xCC10; /* config cmd */
 386
 387         return par->board->met_wait_event(par);
 388 }
 389
 390 static int metronome_init_cmd(struct metronomefb_par *par)
 391 {
 392         int i;
 393         u16 cs;
 394
 395         /* setup init command
 396         we can't immediately set the opcode since the controller
 397         will try parse the command before we've set it all up
 398         so we just set cs here and set the opcode at the end */
 399
 400         cs = 0xCC20;
 401
 402         /* set the args ( 2 bytes ) for init */
 403         i = 0;
 404         par->metromem_cmd->args[i] = 0;
 405         cs += par->metromem_cmd->args[i++];
 406
 407         /* the rest are 0 */
 408         memset((u8 *) (par->metromem_cmd->args + i), 0, (32-i)*2);
 409
 410         par->metromem_cmd->csum = cs;
 411         par->metromem_cmd->opcode = 0xCC20; /* init cmd */
 412
 413         return par->board->met_wait_event(par);
 414 }
 415
 416 static int metronome_init_regs(struct metronomefb_par *par)
 417 {
 418         int res;
 419
 420         res = par->board->setup_io(par);
 421         if (res)
 422                 return res;
 423
 424         res = metronome_powerup_cmd(par);
 425         if (res)
 426                 return res;
 427
 428         res = metronome_config_cmd(par);
 429         if (res)
 430                 return res;
 431
 432         res = metronome_init_cmd(par);
 433
 434         return res;
 435 }
 436
 437 static void metronomefb_dpy_update(struct metronomefb_par *par)
 438 {
 439         int fbsize;
 440         u16 cksum;
 441         unsigned char *buf = (unsigned char __force *)par->info->screen_base;
 442
 443         fbsize = par->info->fix.smem_len;
 444         /* copy from vm to metromem */
 445         memcpy(par->metromem_img, buf, fbsize);
 446
 447         cksum = calc_img_cksum((u16 *) par->metromem_img, fbsize/2);
 448         *((u16 *)(par->metromem_img) + fbsize/2) = cksum;
 449         metronome_display_cmd(par);
 450 }
 451
 452 static u16 metronomefb_dpy_update_page(struct metronomefb_par *par, int index)
 453 {
 454         int i;
 455         u16 csum = 0;
 456         u16 *buf = (u16 __force *)(par->info->screen_base + index);
 457         u16 *img = (u16 *)(par->metromem_img + index);
 458
 459         /* swizzle from vm to metromem and recalc cksum at the same time*/
 460         for (i = 0; i < PAGE_SIZE/2; i++) {
 461                 *(img + i) = (buf[i] << 5) & 0xE0E0;
 462                 csum += *(img + i);
 463         }
 464         return csum;
 465 }
 466
 467 /* this is called back from the deferred io workqueue */
 468 static void metronomefb_dpy_deferred_io(struct fb_info *info, struct list_head *pagereflist)
 469 {
 470         u16 cksum;
 471         struct fb_deferred_io_pageref *pageref;
 472         struct metronomefb_par *par = info->par;
 473
 474         /* walk the written page list and swizzle the data */
 475         list_for_each_entry(pageref, pagereflist, list) {
 476                 unsigned long pgoffset = pageref->offset >> PAGE_SHIFT;
 477                 cksum = metronomefb_dpy_update_page(par, pageref->offset);
 478                 par->metromem_img_csum -= par->csum_table[pgoffset];
 479                 par->csum_table[pgoffset] = cksum;
 480                 par->metromem_img_csum += cksum;
 481         }
 482
 483         metronome_display_cmd(par);
 484 }
 485
 486 static void metronomefb_fillrect(struct fb_info *info,
 487                                    const struct fb_fillrect *rect)
 488 {
 489         struct metronomefb_par *par = info->par;
 490
 491         sys_fillrect(info, rect);
 492         metronomefb_dpy_update(par);
 493 }
 494
 495 static void metronomefb_copyarea(struct fb_info *info,
 496                                    const struct fb_copyarea *area)
 497 {
 498         struct metronomefb_par *par = info->par;
 499
 500         sys_copyarea(info, area);
 501         metronomefb_dpy_update(par);
 502 }
 503
 504 static void metronomefb_imageblit(struct fb_info *info,
 505                                 const struct fb_image *image)
 506 {
 507         struct metronomefb_par *par = info->par;
 508
 509         sys_imageblit(info, image);
 510         metronomefb_dpy_update(par);
 511 }
 512
 513 /*
 514  * this is the slow path from userspace. they can seek and write to
 515  * the fb. it is based on fb_sys_write
 516  */
 517 static ssize_t metronomefb_write(struct fb_info *info, const char __user *buf,
 518                                 size_t count, loff_t *ppos)
 519 {
 520         struct metronomefb_par *par = info->par;
 521         unsigned long p = *ppos;
 522         void *dst;
 523         int err = 0;
 524         unsigned long total_size;
 525
 526         if (info->state != FBINFO_STATE_RUNNING)
 527                 return -EPERM;
 528
 529         total_size = info->fix.smem_len;
 530
 531         if (p > total_size)
 532                 return -EFBIG;
 533
 534         if (count > total_size) {
 535                 err = -EFBIG;
 536                 count = total_size;
 537         }
 538
 539         if (count + p > total_size) {
 540                 if (!err)
 541                         err = -ENOSPC;
 542
 543                 count = total_size - p;
 544         }
 545
 546         dst = (void __force *)(info->screen_base + p);
 547
 548         if (copy_from_user(dst, buf, count))
 549                 err = -EFAULT;
 550
 551         if  (!err)
 552                 *ppos += count;
 553
 554         metronomefb_dpy_update(par);
 555
 556         return (err) ? err : count;
 557 }
 558
 559 static const struct fb_ops metronomefb_ops = {
 560         .owner          = THIS_MODULE,
 561         .fb_write       = metronomefb_write,
 562         .fb_fillrect    = metronomefb_fillrect,
 563         .fb_copyarea    = metronomefb_copyarea,
 564         .fb_imageblit   = metronomefb_imageblit,
 565         .fb_mmap        = fb_deferred_io_mmap,
 566 };
 567
 568 static struct fb_deferred_io metronomefb_defio = {
 569         .delay                  = HZ,
 570         .sort_pagereflist       = true,
 571         .deferred_io            = metronomefb_dpy_deferred_io,
 572 };
 573
 574 static int metronomefb_probe(struct platform_device *dev)
 575 {
 576         struct fb_info *info;
 577         struct metronome_board *board;
 578         int retval = -ENOMEM;
 579         int videomemorysize;
 580         unsigned char *videomemory;
 581         struct metronomefb_par *par;
 582         const struct firmware *fw_entry;
 583         int i;
 584         int panel_type;
 585         int fw, fh;
 586         int epd_dt_index;
 587
 588         /* pick up board specific routines */
 589         board = dev->dev.platform_data;
 590         if (!board)
 591                 return -EINVAL;
 592
 593         /* try to count device specific driver, if can't, platform recalls */
 594         if (!try_module_get(board->owner))
 595                 return -ENODEV;
 596
 597         info = framebuffer_alloc(sizeof(struct metronomefb_par), &dev->dev);
 598         if (!info)
 599                 goto err;
 600
 601         /* we have two blocks of memory.
 602         info->screen_base which is vm, and is the fb used by apps.
 603         par->metromem which is physically contiguous memory and
 604         contains the display controller commands, waveform,
 605         processed image data and padding. this is the data pulled
 606         by the device's LCD controller and pushed to Metronome.
 607         the metromem memory is allocated by the board driver and
 608         is provided to us */
 609
 610         panel_type = board->get_panel_type();
 611         switch (panel_type) {
 612         case 6:
 613                 epd_dt_index = 0;
 614                 break;
 615         case 8:
 616                 epd_dt_index = 1;
 617                 break;
 618         case 97:
 619                 epd_dt_index = 2;
 620                 break;
 621         default:
 622                 dev_err(&dev->dev, "Unexpected panel type. Defaulting to 6\n");
 623                 epd_dt_index = 0;
 624                 break;
 625         }
 626
 627         fw = epd_frame_table[epd_dt_index].fw;
 628         fh = epd_frame_table[epd_dt_index].fh;
 629
 630         /* we need to add a spare page because our csum caching scheme walks
 631          * to the end of the page */
 632         videomemorysize = PAGE_SIZE + (fw * fh);
 633         videomemory = vzalloc(videomemorysize);
 634         if (!videomemory)
 635                 goto err_fb_rel;
 636
 637         info->screen_base = (char __force __iomem *)videomemory;
 638         info->fbops = &metronomefb_ops;
 639
 640         metronomefb_fix.line_length = fw;
 641         metronomefb_var.xres = fw;
 642         metronomefb_var.yres = fh;
 643         metronomefb_var.xres_virtual = fw;
 644         metronomefb_var.yres_virtual = fh;
 645         info->var = metronomefb_var;
 646         info->fix = metronomefb_fix;
 647         info->fix.smem_len = videomemorysize;
 648         par = info->par;
 649         par->info = info;
 650         par->board = board;
 651         par->dt = epd_dt_index;
 652         init_waitqueue_head(&par->waitq);
 653
 654         /* this table caches per page csum values. */
 655         par->csum_table = vmalloc(videomemorysize/PAGE_SIZE);
 656         if (!par->csum_table)
 657                 goto err_vfree;
 658
 659         /* the physical framebuffer that we use is setup by
 660          * the platform device driver. It will provide us
 661          * with cmd, wfm and image memory in a contiguous area. */
 662         retval = board->setup_fb(par);
 663         if (retval) {
 664                 dev_err(&dev->dev, "Failed to setup fb\n");
 665                 goto err_csum_table;
 666         }
 667
 668         /* after this point we should have a framebuffer */
 669         if ((!par->metromem_wfm) ||  (!par->metromem_img) ||
 670                 (!par->metromem_dma)) {
 671                 dev_err(&dev->dev, "fb access failure\n");
 672                 retval = -EINVAL;
 673                 goto err_csum_table;
 674         }
 675
 676         info->fix.smem_start = par->metromem_dma;
 677
 678         /* load the waveform in. assume mode 3, temp 31 for now
 679                 a) request the waveform file from userspace
 680                 b) process waveform and decode into metromem */
 681         retval = request_firmware(&fw_entry, "metronome.wbf", &dev->dev);
 682         if (retval < 0) {
 683                 dev_err(&dev->dev, "Failed to get waveform\n");
 684                 goto err_csum_table;
 685         }
 686
 687         retval = load_waveform((u8 *) fw_entry->data, fw_entry->size, 3, 31,
 688                                 par);
 689         release_firmware(fw_entry);
 690         if (retval < 0) {
 691                 dev_err(&dev->dev, "Failed processing waveform\n");
 692                 goto err_csum_table;
 693         }
 694
 695         retval = board->setup_irq(info);
 696         if (retval)
 697                 goto err_csum_table;
 698
 699         retval = metronome_init_regs(par);
 700         if (retval < 0)
 701                 goto err_free_irq;
 702
 703         info->flags = FBINFO_FLAG_DEFAULT | FBINFO_VIRTFB;
 704
 705         info->fbdefio = &metronomefb_defio;
 706         fb_deferred_io_init(info);
 707
 708         retval = fb_alloc_cmap(&info->cmap, 8, 0);
 709         if (retval < 0) {
 710                 dev_err(&dev->dev, "Failed to allocate colormap\n");
 711                 goto err_free_irq;
 712         }
 713
 714         /* set cmap */
 715         for (i = 0; i < 8; i++)
 716                 info->cmap.red[i] = (((2*i)+1)*(0xFFFF))/16;
 717         memcpy(info->cmap.green, info->cmap.red, sizeof(u16)*8);
 718         memcpy(info->cmap.blue, info->cmap.red, sizeof(u16)*8);
 719
 720         retval = register_framebuffer(info);
 721         if (retval < 0)
 722                 goto err_cmap;
 723
 724         platform_set_drvdata(dev, info);
 725
 726         dev_dbg(&dev->dev,
 727                 "fb%d: Metronome frame buffer device, using %dK of video"
 728                 " memory\n", info->node, videomemorysize >> 10);
 729
 730         return 0;
 731
 732 err_cmap:
 733         fb_dealloc_cmap(&info->cmap);
 734 err_free_irq:
 735         board->cleanup(par);
 736 err_csum_table:
 737         vfree(par->csum_table);
 738 err_vfree:
 739         vfree(videomemory);
 740 err_fb_rel:
 741         framebuffer_release(info);
 742 err:
 743         module_put(board->owner);
 744         return retval;
 745 }
 746
 747 static int metronomefb_remove(struct platform_device *dev)
 748 {
 749         struct fb_info *info = platform_get_drvdata(dev);
 750
 751         if (info) {
 752                 struct metronomefb_par *par = info->par;
 753
 754                 unregister_framebuffer(info);
 755                 fb_deferred_io_cleanup(info);
 756                 fb_dealloc_cmap(&info->cmap);
 757                 par->board->cleanup(par);
 758                 vfree(par->csum_table);
 759                 vfree((void __force *)info->screen_base);
 760                 module_put(par->board->owner);
 761                 dev_dbg(&dev->dev, "calling release\n");
 762                 framebuffer_release(info);
 763         }
 764         return 0;
 765 }
 766
 767 static struct platform_driver metronomefb_driver = {
 768         .probe  = metronomefb_probe,
 769         .remove = metronomefb_remove,
 770         .driver = {
 771                 .name   = "metronomefb",
 772         },
 773 };
 774 module_platform_driver(metronomefb_driver);
 775
 776 module_param(user_wfm_size, uint, 0);
 777 MODULE_PARM_DESC(user_wfm_size, "Set custom waveform size");
 778
 779 MODULE_DESCRIPTION("fbdev driver for Metronome controller");
 780 MODULE_AUTHOR("Jaya Kumar");
 781 MODULE_LICENSE("GPL");