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