drivers/staging/udlfb/udlfb.c

   1 /*****************************************************************************
   2  *                          DLFB Kernel Driver                               *
   3  *                            Version 0.2 (udlfb)                            *
   4  *             (C) 2009 Roberto De Ioris <roberto@unbit.it>                  *
   5  *                                                                           *
   6  *     This file is licensed under the GPLv2. See COPYING in the package.    *
   7  * Based on the amazing work of Florian Echtler and libdlo 0.1               *
   8  *                                                                           *
   9  *                                                                           *
  10  * 10.06.09 release 0.2.3 (edid ioctl, fallback for unsupported modes)       *
  11  * 05.06.09 release 0.2.2 (real screen blanking, rle compression, double buffer) *
  12  * 31.05.09 release 0.2                                                      *
  13  * 22.05.09 First public (ugly) release                                      *
  14  *****************************************************************************/
  15
  16 #include <linux/module.h>
  17 #include <linux/kernel.h>
  18 #include <linux/init.h>
  19 #include <linux/usb.h>
  20 #include <linux/uaccess.h>
  21 #include <linux/mm.h>
  22 #include <linux/fb.h>
  23 #include <linux/mutex.h>
  24 #include <linux/vmalloc.h>
  25
  26 #include "udlfb.h"
  27
  28 #define DRIVER_VERSION "DLFB 0.2"
  29
  30 /* memory functions taken from vfb */
  31
  32 static void *rvmalloc(unsigned long size)
  33 {
  34         void *mem;
  35         unsigned long adr;
  36
  37         size = PAGE_ALIGN(size);
  38         mem = vmalloc_32(size);
  39         if (!mem)
  40                 return NULL;
  41
  42         memset(mem, 0, size);   /* Clear the ram out, no junk to the user */
  43         adr = (unsigned long)mem;
  44         while (size > 0) {
  45                 SetPageReserved(vmalloc_to_page((void *)adr));
  46                 adr += PAGE_SIZE;
  47                 size -= PAGE_SIZE;
  48         }
  49
  50         return mem;
  51 }
  52
  53 static void rvfree(void *mem, unsigned long size)
  54 {
  55         unsigned long adr;
  56
  57         if (!mem)
  58                 return;
  59
  60         adr = (unsigned long)mem;
  61         while ((long)size > 0) {
  62                 ClearPageReserved(vmalloc_to_page((void *)adr));
  63                 adr += PAGE_SIZE;
  64                 size -= PAGE_SIZE;
  65         }
  66         vfree(mem);
  67 }
  68
  69 static int dlfb_mmap(struct fb_info *info, struct vm_area_struct *vma)
  70 {
  71         unsigned long start = vma->vm_start;
  72         unsigned long size = vma->vm_end - vma->vm_start;
  73         unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
  74         unsigned long page, pos;
  75
  76         printk("MMAP: %lu %u\n", offset + size, info->fix.smem_len);
  77
  78         if (offset + size > info->fix.smem_len)
  79                 return -EINVAL;
  80
  81         pos = (unsigned long)info->fix.smem_start + offset;
  82
  83         while (size > 0) {
  84                 page = vmalloc_to_pfn((void *)pos);
  85                 if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED))
  86                         return -EAGAIN;
  87
  88                 start += PAGE_SIZE;
  89                 pos += PAGE_SIZE;
  90                 if (size > PAGE_SIZE)
  91                         size -= PAGE_SIZE;
  92                 else
  93                         size = 0;
  94         }
  95
  96         vma->vm_flags |= VM_RESERVED;   /* avoid to swap out this VMA */
  97         return 0;
  98
  99 }
 100
 101 /* ioctl structure */
 102 struct dloarea {
 103         int x, y;
 104         int w, h;
 105         int x2, y2;
 106 };
 107
 108 /*
 109 static struct usb_device_id id_table [] = {
 110         { USB_DEVICE(0x17e9, 0x023d) },
 111         { }
 112 };
 113 */
 114
 115 static struct usb_device_id id_table[] = {
 116         {.idVendor = 0x17e9, .match_flags = USB_DEVICE_ID_MATCH_VENDOR,},
 117         {},
 118 };
 119 MODULE_DEVICE_TABLE(usb, id_table);
 120
 121 static struct usb_driver dlfb_driver;
 122
 123 // thanks to Henrik Bjerregaard Pedersen for this function
 124 static char *rle_compress16(uint16_t * src, char *dst, int rem)
 125 {
 126
 127         int rl;
 128         uint16_t pix0;
 129         char *end_if_raw = dst + 6 + 2 * rem;
 130
 131         dst += 6;               // header will be filled in if RLE is worth it
 132
 133         while (rem && dst < end_if_raw) {
 134                 char *start = (char *)src;
 135
 136                 pix0 = *src++;
 137                 rl = 1;
 138                 rem--;
 139                 while (rem && *src == pix0)
 140                         rem--, rl++, src++;
 141                 *dst++ = rl;
 142                 *dst++ = start[1];
 143                 *dst++ = start[0];
 144         }
 145
 146         return dst;
 147 }
 148
 149 /*
 150 Thanks to Henrik Bjerregaard Pedersen for rle implementation and code refactoring.
 151 Next step is huffman compression.
 152 */
 153
 154 static int
 155 image_blit(struct dlfb_data *dev_info, int x, int y, int width, int height,
 156            char *data)
 157 {
 158
 159         int i, j, base;
 160         int rem = width;
 161         int ret;
 162
 163         int firstdiff, thistime;
 164
 165         char *bufptr;
 166
 167         if (x + width > dev_info->info->var.xres)
 168                 return -EINVAL;
 169
 170         if (y + height > dev_info->info->var.yres)
 171                 return -EINVAL;
 172
 173         mutex_lock(&dev_info->bulk_mutex);
 174
 175         base =
 176             dev_info->base16 + ((dev_info->info->var.xres * 2 * y) + (x * 2));
 177
 178         data += (dev_info->info->var.xres * 2 * y) + (x * 2);
 179
 180         /* printk("IMAGE_BLIT\n"); */
 181
 182         bufptr = dev_info->buf;
 183
 184         for (i = y; i < y + height; i++) {
 185
 186                 if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 187                         ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 188                         bufptr = dev_info->buf;
 189                 }
 190
 191                 rem = width;
 192
 193                 /* printk("WRITING LINE %d\n", i); */
 194
 195                 while (rem) {
 196
 197                         if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 198                                 ret =
 199                                     dlfb_bulk_msg(dev_info,
 200                                                   bufptr - dev_info->buf);
 201                                 bufptr = dev_info->buf;
 202                         }
 203                         // number of pixels to consider this time
 204                         thistime = rem;
 205                         if (thistime > 255)
 206                                 thistime = 255;
 207
 208                         // find position of first pixel that has changed
 209                         firstdiff = -1;
 210                         for (j = 0; j < thistime * 2; j++) {
 211                                 if (dev_info->backing_buffer
 212                                     [base - dev_info->base16 + j] != data[j]) {
 213                                         firstdiff = j / 2;
 214                                         break;
 215                                 }
 216                         }
 217
 218                         if (firstdiff >= 0) {
 219                                 char *end_of_rle;
 220
 221                                 end_of_rle =
 222                                     rle_compress16((uint16_t *) (data +
 223                                                                  firstdiff * 2),
 224                                                    bufptr,
 225                                                    thistime - firstdiff);
 226
 227                                 if (end_of_rle <
 228                                     bufptr + 6 + 2 * (thistime - firstdiff)) {
 229                                         bufptr[0] = 0xAF;
 230                                         bufptr[1] = 0x69;
 231
 232                                         bufptr[2] =
 233                                             (char)((base +
 234                                                     firstdiff * 2) >> 16);
 235                                         bufptr[3] =
 236                                             (char)((base + firstdiff * 2) >> 8);
 237                                         bufptr[4] =
 238                                             (char)(base + firstdiff * 2);
 239                                         bufptr[5] = thistime - firstdiff;
 240
 241                                         bufptr = end_of_rle;
 242
 243                                 } else {
 244                                         // fallback to raw (or some other encoding?)
 245                                         *bufptr++ = 0xAF;
 246                                         *bufptr++ = 0x68;
 247
 248                                         *bufptr++ =
 249                                             (char)((base +
 250                                                     firstdiff * 2) >> 16);
 251                                         *bufptr++ =
 252                                             (char)((base + firstdiff * 2) >> 8);
 253                                         *bufptr++ =
 254                                             (char)(base + firstdiff * 2);
 255                                         *bufptr++ = thistime - firstdiff;
 256                                         // PUT COMPRESSION HERE
 257                                         for (j = firstdiff * 2;
 258                                              j < thistime * 2; j += 2) {
 259                                                 *bufptr++ = data[j + 1];
 260                                                 *bufptr++ = data[j];
 261                                         }
 262                                 }
 263                         }
 264
 265                         base += thistime * 2;
 266                         data += thistime * 2;
 267                         rem -= thistime;
 268                 }
 269
 270                 memcpy(dev_info->backing_buffer + (base - dev_info->base16) -
 271                        (width * 2), data - (width * 2), width * 2);
 272
 273                 base += (dev_info->info->var.xres * 2) - (width * 2);
 274                 data += (dev_info->info->var.xres * 2) - (width * 2);
 275
 276         }
 277
 278         if (bufptr > dev_info->buf) {
 279                 ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 280         }
 281
 282         mutex_unlock(&dev_info->bulk_mutex);
 283
 284         return base;
 285
 286 }
 287
 288 static int
 289 draw_rect(struct dlfb_data *dev_info, int x, int y, int width, int height,
 290           unsigned char red, unsigned char green, unsigned char blue)
 291 {
 292
 293         int i, j, base;
 294         int ret;
 295         unsigned short col =
 296             (((((red) & 0xF8) | ((green) >> 5)) & 0xFF) << 8) +
 297             (((((green) & 0x1C) << 3) | ((blue) >> 3)) & 0xFF);
 298         int rem = width;
 299
 300         char *bufptr;
 301
 302         if (x + width > dev_info->info->var.xres)
 303                 return -EINVAL;
 304
 305         if (y + height > dev_info->info->var.yres)
 306                 return -EINVAL;
 307
 308         mutex_lock(&dev_info->bulk_mutex);
 309
 310         base = dev_info->base16 + (dev_info->info->var.xres * 2 * y) + (x * 2);
 311
 312         bufptr = dev_info->buf;
 313
 314         for (i = y; i < y + height; i++) {
 315
 316                 for (j = 0; j < width * 2; j += 2) {
 317                         dev_info->backing_buffer[base - dev_info->base16 + j] =
 318                             (char)(col >> 8);
 319                         dev_info->backing_buffer[base - dev_info->base16 + j +
 320                                                  1] = (char)(col);
 321                 }
 322                 if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 323                         ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 324                         bufptr = dev_info->buf;
 325                 }
 326
 327                 rem = width;
 328
 329                 while (rem) {
 330
 331                         if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 332                                 ret =
 333                                     dlfb_bulk_msg(dev_info,
 334                                                   bufptr - dev_info->buf);
 335                                 bufptr = dev_info->buf;
 336                         }
 337
 338                         *bufptr++ = 0xAF;
 339                         *bufptr++ = 0x69;
 340
 341                         *bufptr++ = (char)(base >> 16);
 342                         *bufptr++ = (char)(base >> 8);
 343                         *bufptr++ = (char)(base);
 344
 345                         if (rem > 255) {
 346                                 *bufptr++ = 255;
 347                                 *bufptr++ = 255;
 348                                 rem -= 255;
 349                                 base += 255 * 2;
 350                         } else {
 351                                 *bufptr++ = rem;
 352                                 *bufptr++ = rem;
 353                                 base += rem * 2;
 354                                 rem = 0;
 355                         }
 356
 357                         *bufptr++ = (char)(col >> 8);
 358                         *bufptr++ = (char)(col);
 359
 360                 }
 361
 362                 base += (dev_info->info->var.xres * 2) - (width * 2);
 363
 364         }
 365
 366         if (bufptr > dev_info->buf)
 367                 ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 368
 369         mutex_unlock(&dev_info->bulk_mutex);
 370
 371         return 1;
 372 }
 373
 374 static void swapfb(struct dlfb_data *dev_info)
 375 {
 376
 377         int tmpbase;
 378         char *bufptr;
 379
 380         mutex_lock(&dev_info->bulk_mutex);
 381
 382         tmpbase = dev_info->base16;
 383
 384         dev_info->base16 = dev_info->base16d;
 385         dev_info->base16d = tmpbase;
 386
 387         bufptr = dev_info->buf;
 388
 389         bufptr = dlfb_set_register(bufptr, 0xFF, 0x00);
 390
 391         // set addresses
 392         bufptr =
 393             dlfb_set_register(bufptr, 0x20, (char)(dev_info->base16 >> 16));
 394         bufptr = dlfb_set_register(bufptr, 0x21, (char)(dev_info->base16 >> 8));
 395         bufptr = dlfb_set_register(bufptr, 0x22, (char)(dev_info->base16));
 396
 397         bufptr = dlfb_set_register(bufptr, 0xFF, 0x00);
 398
 399         dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 400
 401         mutex_unlock(&dev_info->bulk_mutex);
 402 }
 403
 404 static int copyfb(struct dlfb_data *dev_info)
 405 {
 406         int base;
 407         int source;
 408         int rem;
 409         int i, ret;
 410
 411         char *bufptr;
 412
 413         base = dev_info->base16d;
 414
 415         mutex_lock(&dev_info->bulk_mutex);
 416
 417         source = dev_info->base16;
 418
 419         bufptr = dev_info->buf;
 420
 421         for (i = 0; i < dev_info->info->var.yres; i++) {
 422
 423                 if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 424                         ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 425                         bufptr = dev_info->buf;
 426                 }
 427
 428                 rem = dev_info->info->var.xres;
 429
 430                 while (rem) {
 431
 432                         if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 433                                 ret =
 434                                     dlfb_bulk_msg(dev_info,
 435                                                   bufptr - dev_info->buf);
 436                                 bufptr = dev_info->buf;
 437
 438                         }
 439
 440                         *bufptr++ = 0xAF;
 441                         *bufptr++ = 0x6A;
 442
 443                         *bufptr++ = (char)(base >> 16);
 444                         *bufptr++ = (char)(base >> 8);
 445                         *bufptr++ = (char)(base);
 446
 447                         if (rem > 255) {
 448                                 *bufptr++ = 255;
 449                                 *bufptr++ = (char)(source >> 16);
 450                                 *bufptr++ = (char)(source >> 8);
 451                                 *bufptr++ = (char)(source);
 452
 453                                 rem -= 255;
 454                                 base += 255 * 2;
 455                                 source += 255 * 2;
 456
 457                         } else {
 458                                 *bufptr++ = rem;
 459                                 *bufptr++ = (char)(source >> 16);
 460                                 *bufptr++ = (char)(source >> 8);
 461                                 *bufptr++ = (char)(source);
 462
 463                                 base += rem * 2;
 464                                 source += rem * 2;
 465                                 rem = 0;
 466                         }
 467                 }
 468         }
 469
 470         if (bufptr > dev_info->buf)
 471                 ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 472
 473         mutex_unlock(&dev_info->bulk_mutex);
 474
 475         return 1;
 476
 477 }
 478
 479 static int
 480 copyarea(struct dlfb_data *dev_info, int dx, int dy, int sx, int sy,
 481          int width, int height)
 482 {
 483         int base;
 484         int source;
 485         int rem;
 486         int i, ret;
 487
 488         char *bufptr;
 489
 490         if (dx + width > dev_info->info->var.xres)
 491                 return -EINVAL;
 492
 493         if (dy + height > dev_info->info->var.yres)
 494                 return -EINVAL;
 495
 496         mutex_lock(&dev_info->bulk_mutex);
 497
 498         base =
 499             dev_info->base16 + (dev_info->info->var.xres * 2 * dy) + (dx * 2);
 500         source = (dev_info->info->var.xres * 2 * sy) + (sx * 2);
 501
 502         bufptr = dev_info->buf;
 503
 504         for (i = sy; i < sy + height; i++) {
 505
 506                 memcpy(dev_info->backing_buffer + base - dev_info->base16,
 507                        dev_info->backing_buffer + source, width * 2);
 508
 509                 if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 510                         ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 511                         bufptr = dev_info->buf;
 512                 }
 513
 514                 rem = width;
 515
 516                 while (rem) {
 517
 518                         if (dev_info->bufend - bufptr < BUF_HIGH_WATER_MARK) {
 519                                 ret =
 520                                     dlfb_bulk_msg(dev_info,
 521                                                   bufptr - dev_info->buf);
 522                                 bufptr = dev_info->buf;
 523                         }
 524
 525                         *bufptr++ = 0xAF;
 526                         *bufptr++ = 0x6A;
 527
 528                         *bufptr++ = (char)(base >> 16);
 529                         *bufptr++ = (char)(base >> 8);
 530                         *bufptr++ = (char)(base);
 531
 532                         if (rem > 255) {
 533                                 *bufptr++ = 255;
 534                                 *bufptr++ = (char)(source >> 16);
 535                                 *bufptr++ = (char)(source >> 8);
 536                                 *bufptr++ = (char)(source);
 537
 538                                 rem -= 255;
 539                                 base += 255 * 2;
 540                                 source += 255 * 2;
 541
 542                         } else {
 543                                 *bufptr++ = rem;
 544                                 *bufptr++ = (char)(source >> 16);
 545                                 *bufptr++ = (char)(source >> 8);
 546                                 *bufptr++ = (char)(source);
 547
 548                                 base += rem * 2;
 549                                 source += rem * 2;
 550                                 rem = 0;
 551                         }
 552                 }
 553
 554                 base += (dev_info->info->var.xres * 2) - (width * 2);
 555                 source += (dev_info->info->var.xres * 2) - (width * 2);
 556         }
 557
 558         if (bufptr > dev_info->buf)
 559                 ret = dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 560
 561         mutex_unlock(&dev_info->bulk_mutex);
 562
 563         return 1;
 564 }
 565
 566 static void dlfb_copyarea(struct fb_info *info, const struct fb_copyarea *area)
 567 {
 568
 569         struct dlfb_data *dev = info->par;
 570
 571         copyarea(dev, area->dx, area->dy, area->sx, area->sy, area->width,
 572                  area->height);
 573
 574         /* printk("COPY AREA %d %d %d %d %d %d !!!\n", area->dx, area->dy, area->sx, area->sy, area->width, area->height); */
 575
 576 }
 577
 578 static void dlfb_imageblit(struct fb_info *info, const struct fb_image *image)
 579 {
 580
 581         int ret;
 582         struct dlfb_data *dev = info->par;
 583         /* printk("IMAGE BLIT (1) %d %d %d %d DEPTH %d {%p}!!!\n", image->dx, image->dy, image->width, image->height, image->depth, dev->udev); */
 584         cfb_imageblit(info, image);
 585         ret =
 586             image_blit(dev, image->dx, image->dy, image->width, image->height,
 587                        info->screen_base);
 588         /* printk("IMAGE BLIT (2) %d %d %d %d DEPTH %d {%p} %d!!!\n", image->dx, image->dy, image->width, image->height, image->depth, dev->udev, ret); */
 589 }
 590
 591 static void dlfb_fillrect(struct fb_info *info,
 592                           const struct fb_fillrect *region)
 593 {
 594
 595         unsigned char red, green, blue;
 596         struct dlfb_data *dev = info->par;
 597
 598         memcpy(&red, &region->color, 1);
 599         memcpy(&green, &region->color + 1, 1);
 600         memcpy(&blue, &region->color + 2, 1);
 601         draw_rect(dev, region->dx, region->dy, region->width, region->height,
 602                   red, green, blue);
 603         /* printk("FILL RECT %d %d !!!\n", region->dx, region->dy); */
 604
 605 }
 606
 607 static int dlfb_ioctl(struct fb_info *info, unsigned int cmd, unsigned long arg)
 608 {
 609
 610         struct dlfb_data *dev_info = info->par;
 611         struct dloarea *area = NULL;
 612
 613         if (cmd == 0xAD) {
 614                 char *edid = (char *)arg;
 615                 dlfb_edid(dev_info);
 616                 if (copy_to_user(edid, dev_info->edid, 128)) {
 617                         return -EFAULT;
 618                 }
 619                 return 0;
 620         }
 621
 622         if (cmd == 0xAA || cmd == 0xAB || cmd == 0xAC) {
 623
 624                 area = (struct dloarea *)arg;
 625
 626                 if (area->x < 0)
 627                         area->x = 0;
 628
 629                 if (area->x > info->var.xres)
 630                         area->x = info->var.xres;
 631
 632                 if (area->y < 0)
 633                         area->y = 0;
 634
 635                 if (area->y > info->var.yres)
 636                         area->y = info->var.yres;
 637         }
 638
 639         if (cmd == 0xAA) {
 640                 image_blit(dev_info, area->x, area->y, area->w, area->h,
 641                            info->screen_base);
 642         }
 643         if (cmd == 0xAC) {
 644                 copyfb(dev_info);
 645                 image_blit(dev_info, area->x, area->y, area->w, area->h,
 646                            info->screen_base);
 647                 swapfb(dev_info);
 648         } else if (cmd == 0xAB) {
 649
 650                 if (area->x2 < 0)
 651                         area->x2 = 0;
 652
 653                 if (area->y2 < 0)
 654                         area->y2 = 0;
 655
 656                 copyarea(dev_info,
 657                          area->x2, area->y2, area->x, area->y, area->w,
 658                          area->h);
 659         }
 660         return 0;
 661 }
 662
 663 /* taken from vesafb */
 664
 665 static int
 666 dlfb_setcolreg(unsigned regno, unsigned red, unsigned green,
 667                unsigned blue, unsigned transp, struct fb_info *info)
 668 {
 669         int err = 0;
 670
 671         if (regno >= info->cmap.len)
 672                 return 1;
 673
 674         if (regno < 16) {
 675                 if (info->var.red.offset == 10) {
 676                         /* 1:5:5:5 */
 677                         ((u32 *) (info->pseudo_palette))[regno] =
 678                             ((red & 0xf800) >> 1) |
 679                             ((green & 0xf800) >> 6) | ((blue & 0xf800) >> 11);
 680                 } else {
 681                         /* 0:5:6:5 */
 682                         ((u32 *) (info->pseudo_palette))[regno] =
 683                             ((red & 0xf800)) |
 684                             ((green & 0xfc00) >> 5) | ((blue & 0xf800) >> 11);
 685                 }
 686         }
 687
 688         return err;
 689 }
 690
 691 static int dlfb_release(struct fb_info *info, int user)
 692 {
 693         struct dlfb_data *dev_info = info->par;
 694         image_blit(dev_info, 0, 0, info->var.xres, info->var.yres,
 695                    info->screen_base);
 696         return 0;
 697 }
 698
 699 static int dlfb_blank(int blank_mode, struct fb_info *info)
 700 {
 701         struct dlfb_data *dev_info = info->par;
 702         char *bufptr = dev_info->buf;
 703
 704         bufptr = dlfb_set_register(bufptr, 0xFF, 0x00);
 705         if (blank_mode != FB_BLANK_UNBLANK) {
 706                 bufptr = dlfb_set_register(bufptr, 0x1F, 0x01);
 707         } else {
 708                 bufptr = dlfb_set_register(bufptr, 0x1F, 0x00);
 709         }
 710         bufptr = dlfb_set_register(bufptr, 0xFF, 0xFF);
 711
 712         dlfb_bulk_msg(dev_info, bufptr - dev_info->buf);
 713
 714         return 0;
 715 }
 716
 717 static struct fb_ops dlfb_ops = {
 718         .fb_setcolreg = dlfb_setcolreg,
 719         .fb_fillrect = dlfb_fillrect,
 720         .fb_copyarea = dlfb_copyarea,
 721         .fb_imageblit = dlfb_imageblit,
 722         .fb_mmap = dlfb_mmap,
 723         .fb_ioctl = dlfb_ioctl,
 724         .fb_release = dlfb_release,
 725         .fb_blank = dlfb_blank,
 726 };
 727
 728 static int
 729 dlfb_probe(struct usb_interface *interface, const struct usb_device_id *id)
 730 {
 731         struct dlfb_data *dev_info;
 732         struct fb_info *info;
 733
 734         int ret;
 735         char rbuf[4];
 736
 737         dev_info = kzalloc(sizeof(*dev_info), GFP_KERNEL);
 738         if (dev_info == NULL) {
 739                 printk("cannot allocate dev_info structure.\n");
 740                 return -ENOMEM;
 741         }
 742
 743         mutex_init(&dev_info->bulk_mutex);
 744
 745         dev_info->udev = usb_get_dev(interface_to_usbdev(interface));
 746         dev_info->interface = interface;
 747
 748         printk("DisplayLink device attached\n");
 749
 750         /* add framebuffer info to usb interface */
 751         usb_set_intfdata(interface, dev_info);
 752
 753         dev_info->buf = kmalloc(BUF_SIZE, GFP_KERNEL);
 754         /* usb_buffer_alloc(dev_info->udev, BUF_SIZE , GFP_KERNEL, &dev_info->tx_urb->transfer_dma); */
 755
 756         if (dev_info->buf == NULL) {
 757                 printk("unable to allocate memory for dlfb commands\n");
 758                 goto out;
 759         }
 760         dev_info->bufend = dev_info->buf + BUF_SIZE;
 761
 762         dev_info->tx_urb = usb_alloc_urb(0, GFP_KERNEL);
 763         usb_fill_bulk_urb(dev_info->tx_urb, dev_info->udev,
 764                           usb_sndbulkpipe(dev_info->udev, 1), dev_info->buf, 0,
 765                           dlfb_bulk_callback, dev_info);
 766
 767         ret =
 768             usb_control_msg(dev_info->udev, usb_rcvctrlpipe(dev_info->udev, 0),
 769                             (0x06), (0x80 | (0x02 << 5)), 0, 0, rbuf, 4, 0);
 770         printk("ret control msg 0: %d %x%x%x%x\n", ret, rbuf[0], rbuf[1],
 771                rbuf[2], rbuf[3]);
 772
 773         dlfb_edid(dev_info);
 774
 775         info = framebuffer_alloc(sizeof(u32) * 256, &dev_info->udev->dev);
 776
 777         if (!info) {
 778                 printk("non posso allocare il framebuffer displaylink");
 779                 goto out;
 780         }
 781
 782         fb_parse_edid(dev_info->edid, &info->var);
 783
 784         printk("EDID XRES %d YRES %d\n", info->var.xres, info->var.yres);
 785
 786         if (dlfb_set_video_mode(dev_info, info->var.xres, info->var.yres) != 0) {
 787                 info->var.xres = 1280;
 788                 info->var.yres = 1024;
 789                 if (dlfb_set_video_mode
 790                     (dev_info, info->var.xres, info->var.yres) != 0) {
 791                         goto out;
 792                 }
 793         }
 794
 795         printk("found valid mode...%d\n", info->var.pixclock);
 796
 797         info->pseudo_palette = info->par;
 798         info->par = dev_info;
 799
 800         dev_info->info = info;
 801
 802         info->flags =
 803             FBINFO_DEFAULT | FBINFO_READS_FAST | FBINFO_HWACCEL_IMAGEBLIT |
 804             FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT;
 805         info->fbops = &dlfb_ops;
 806         info->screen_base = rvmalloc(dev_info->screen_size);
 807
 808         if (info->screen_base == NULL) {
 809                 printk
 810                     ("cannot allocate framebuffer virtual memory of %d bytes\n",
 811                      dev_info->screen_size);
 812                 goto out0;
 813         }
 814
 815         printk("screen base allocated !!!\n");
 816
 817         dev_info->backing_buffer = kzalloc(dev_info->screen_size, GFP_KERNEL);
 818
 819         if (!dev_info->backing_buffer)
 820                 printk("non posso allocare il backing buffer\n");
 821
 822         /* info->var = dev_info->si; */
 823
 824         info->var.bits_per_pixel = 16;
 825         info->var.activate = FB_ACTIVATE_TEST;
 826         info->var.vmode = FB_VMODE_NONINTERLACED;
 827
 828         info->var.red.offset = 11;
 829         info->var.red.length = 5;
 830         info->var.red.msb_right = 0;
 831
 832         info->var.green.offset = 5;
 833         info->var.green.length = 6;
 834         info->var.green.msb_right = 0;
 835
 836         info->var.blue.offset = 0;
 837         info->var.blue.length = 5;
 838         info->var.blue.msb_right = 0;
 839
 840         /* info->var.pixclock =  (10000000 / FB_W * 1000 / FB_H)/2 ; */
 841
 842         info->fix.smem_start = (unsigned long)info->screen_base;
 843         info->fix.smem_len = PAGE_ALIGN(dev_info->screen_size);
 844         if (strlen(dev_info->udev->product) > 15) {
 845                 memcpy(info->fix.id, dev_info->udev->product, 15);
 846         } else {
 847                 memcpy(info->fix.id, dev_info->udev->product,
 848                        strlen(dev_info->udev->product));
 849         }
 850         info->fix.type = FB_TYPE_PACKED_PIXELS;
 851         info->fix.visual = FB_VISUAL_TRUECOLOR;
 852         info->fix.accel = info->flags;
 853         info->fix.line_length = dev_info->line_length;
 854
 855         if (fb_alloc_cmap(&info->cmap, 256, 0) < 0)
 856                 goto out1;
 857
 858         printk("colormap allocated\n");
 859         if (register_framebuffer(info) < 0)
 860                 goto out2;
 861
 862         draw_rect(dev_info, 0, 0, dev_info->info->var.xres,
 863                   dev_info->info->var.yres, 0x30, 0xff, 0x30);
 864
 865         return 0;
 866
 867 out2:
 868         fb_dealloc_cmap(&info->cmap);
 869 out1:
 870         rvfree(info->screen_base, dev_info->screen_size);
 871 out0:
 872         framebuffer_release(info);
 873 out:
 874         usb_set_intfdata(interface, NULL);
 875         usb_put_dev(dev_info->udev);
 876         kfree(dev_info);
 877         return -ENOMEM;
 878
 879 }
 880
 881 static void dlfb_disconnect(struct usb_interface *interface)
 882 {
 883         struct dlfb_data *dev_info = usb_get_intfdata(interface);
 884
 885         mutex_unlock(&dev_info->bulk_mutex);
 886
 887         usb_kill_urb(dev_info->tx_urb);
 888         usb_free_urb(dev_info->tx_urb);
 889         usb_set_intfdata(interface, NULL);
 890         usb_put_dev(dev_info->udev);
 891
 892         if (dev_info->info) {
 893                 unregister_framebuffer(dev_info->info);
 894                 fb_dealloc_cmap(&dev_info->info->cmap);
 895                 rvfree(dev_info->info->screen_base, dev_info->screen_size);
 896                 kfree(dev_info->backing_buffer);
 897                 framebuffer_release(dev_info->info);
 898
 899         }
 900
 901         kfree(dev_info);
 902
 903         printk("DisplayLink device disconnected\n");
 904 }
 905
 906 static struct usb_driver dlfb_driver = {
 907         .name = "udlfb",
 908         .probe = dlfb_probe,
 909         .disconnect = dlfb_disconnect,
 910         .id_table = id_table,
 911 };
 912
 913 static int __init dlfb_init(void)
 914 {
 915         int res;
 916
 917         dlfb_init_modes();
 918
 919         res = usb_register(&dlfb_driver);
 920         if (res)
 921                 err("usb_register failed. Error number %d", res);
 922
 923         printk("VMODES initialized\n");
 924
 925         return res;
 926 }
 927
 928 static void __exit dlfb_exit(void)
 929 {
 930         usb_deregister(&dlfb_driver);
 931 }
 932
 933 module_init(dlfb_init);
 934 module_exit(dlfb_exit);
 935
 936 MODULE_AUTHOR("Roberto De Ioris <roberto@unbit.it>");
 937 MODULE_DESCRIPTION(DRIVER_VERSION);
 938 MODULE_LICENSE("GPL");