git.oblomov.eu Git - linux-2.6/blob - drivers/media/video/et61x251/et61x251_core.c

   1 /***************************************************************************
   2  * V4L2 driver for ET61X[12]51 PC Camera Controllers                       *
   3  *                                                                         *
   4  * Copyright (C) 2006-2007 by Luca Risolia <luca.risolia@studio.unibo.it>  *
   5  *                                                                         *
   6  * This program is free software; you can redistribute it and/or modify    *
   7  * it under the terms of the GNU General Public License as published by    *
   8  * the Free Software Foundation; either version 2 of the License, or       *
   9  * (at your option) any later version.                                     *
  10  *                                                                         *
  11  * This program is distributed in the hope that it will be useful,         *
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of          *
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the           *
  14  * GNU General Public License for more details.                            *
  15  *                                                                         *
  16  * You should have received a copy of the GNU General Public License       *
  17  * along with this program; if not, write to the Free Software             *
  18  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.               *
  19  ***************************************************************************/
  20
  21 #include <linux/module.h>
  22 #include <linux/init.h>
  23 #include <linux/kernel.h>
  24 #include <linux/param.h>
  25 #include <linux/errno.h>
  26 #include <linux/slab.h>
  27 #include <linux/device.h>
  28 #include <linux/fs.h>
  29 #include <linux/delay.h>
  30 #include <linux/compiler.h>
  31 #include <linux/ioctl.h>
  32 #include <linux/poll.h>
  33 #include <linux/stat.h>
  34 #include <linux/mm.h>
  35 #include <linux/vmalloc.h>
  36 #include <linux/page-flags.h>
  37 #include <asm/byteorder.h>
  38 #include <asm/page.h>
  39 #include <asm/uaccess.h>
  40
  41 #include "et61x251.h"
  42
  43 /*****************************************************************************/
  44
  45 #define ET61X251_MODULE_NAME    "V4L2 driver for ET61X[12]51 "                \
  46                                 "PC Camera Controllers"
  47 #define ET61X251_MODULE_AUTHOR  "(C) 2006-2007 Luca Risolia"
  48 #define ET61X251_AUTHOR_EMAIL   "<luca.risolia@studio.unibo.it>"
  49 #define ET61X251_MODULE_LICENSE "GPL"
  50 #define ET61X251_MODULE_VERSION "1:1.09"
  51 #define ET61X251_MODULE_VERSION_CODE  KERNEL_VERSION(1, 1, 9)
  52
  53 /*****************************************************************************/
  54
  55 MODULE_DEVICE_TABLE(usb, et61x251_id_table);
  56
  57 MODULE_AUTHOR(ET61X251_MODULE_AUTHOR " " ET61X251_AUTHOR_EMAIL);
  58 MODULE_DESCRIPTION(ET61X251_MODULE_NAME);
  59 MODULE_VERSION(ET61X251_MODULE_VERSION);
  60 MODULE_LICENSE(ET61X251_MODULE_LICENSE);
  61
  62 static short video_nr[] = {[0 ... ET61X251_MAX_DEVICES-1] = -1};
  63 module_param_array(video_nr, short, NULL, 0444);
  64 MODULE_PARM_DESC(video_nr,
  65                  "\n<-1|n[,...]> Specify V4L2 minor mode number."
  66                  "\n -1 = use next available (default)"
  67                  "\n  n = use minor number n (integer >= 0)"
  68                  "\nYou can specify up to "
  69                  __MODULE_STRING(ET61X251_MAX_DEVICES) " cameras this way."
  70                  "\nFor example:"
  71                  "\nvideo_nr=-1,2,-1 would assign minor number 2 to"
  72                  "\nthe second registered camera and use auto for the first"
  73                  "\none and for every other camera."
  74                  "\n");
  75
  76 static short force_munmap[] = {[0 ... ET61X251_MAX_DEVICES-1] =
  77                                ET61X251_FORCE_MUNMAP};
  78 module_param_array(force_munmap, bool, NULL, 0444);
  79 MODULE_PARM_DESC(force_munmap,
  80                  "\n<0|1[,...]> Force the application to unmap previously"
  81                  "\nmapped buffer memory before calling any VIDIOC_S_CROP or"
  82                  "\nVIDIOC_S_FMT ioctl's. Not all the applications support"
  83                  "\nthis feature. This parameter is specific for each"
  84                  "\ndetected camera."
  85                  "\n 0 = do not force memory unmapping"
  86                  "\n 1 = force memory unmapping (save memory)"
  87                  "\nDefault value is "__MODULE_STRING(ET61X251_FORCE_MUNMAP)"."
  88                  "\n");
  89
  90 static unsigned int frame_timeout[] = {[0 ... ET61X251_MAX_DEVICES-1] =
  91                                        ET61X251_FRAME_TIMEOUT};
  92 module_param_array(frame_timeout, uint, NULL, 0644);
  93 MODULE_PARM_DESC(frame_timeout,
  94                  "\n<n[,...]> Timeout for a video frame in seconds."
  95                  "\nThis parameter is specific for each detected camera."
  96                  "\nDefault value is "
  97                  __MODULE_STRING(ET61X251_FRAME_TIMEOUT)"."
  98                  "\n");
  99
 100 #ifdef ET61X251_DEBUG
 101 static unsigned short debug = ET61X251_DEBUG_LEVEL;
 102 module_param(debug, ushort, 0644);
 103 MODULE_PARM_DESC(debug,
 104                  "\n<n> Debugging information level, from 0 to 3:"
 105                  "\n0 = none (use carefully)"
 106                  "\n1 = critical errors"
 107                  "\n2 = significant informations"
 108                  "\n3 = more verbose messages"
 109                  "\nLevel 3 is useful for testing only, when only "
 110                  "one device is used."
 111                  "\nDefault value is "__MODULE_STRING(ET61X251_DEBUG_LEVEL)"."
 112                  "\n");
 113 #endif
 114
 115 /*****************************************************************************/
 116
 117 static u32
 118 et61x251_request_buffers(struct et61x251_device* cam, u32 count,
 119                          enum et61x251_io_method io)
 120 {
 121         struct v4l2_pix_format* p = &(cam->sensor.pix_format);
 122         struct v4l2_rect* r = &(cam->sensor.cropcap.bounds);
 123         const size_t imagesize = cam->module_param.force_munmap ||
 124                                  io == IO_READ ?
 125                                  (p->width * p->height * p->priv) / 8 :
 126                                  (r->width * r->height * p->priv) / 8;
 127         void* buff = NULL;
 128         u32 i;
 129
 130         if (count > ET61X251_MAX_FRAMES)
 131                 count = ET61X251_MAX_FRAMES;
 132
 133         cam->nbuffers = count;
 134         while (cam->nbuffers > 0) {
 135                 if ((buff = vmalloc_32_user(cam->nbuffers *
 136                                             PAGE_ALIGN(imagesize))))
 137                         break;
 138                 cam->nbuffers--;
 139         }
 140
 141         for (i = 0; i < cam->nbuffers; i++) {
 142                 cam->frame[i].bufmem = buff + i*PAGE_ALIGN(imagesize);
 143                 cam->frame[i].buf.index = i;
 144                 cam->frame[i].buf.m.offset = i*PAGE_ALIGN(imagesize);
 145                 cam->frame[i].buf.length = imagesize;
 146                 cam->frame[i].buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 147                 cam->frame[i].buf.sequence = 0;
 148                 cam->frame[i].buf.field = V4L2_FIELD_NONE;
 149                 cam->frame[i].buf.memory = V4L2_MEMORY_MMAP;
 150                 cam->frame[i].buf.flags = 0;
 151         }
 152
 153         return cam->nbuffers;
 154 }
 155
 156
 157 static void et61x251_release_buffers(struct et61x251_device* cam)
 158 {
 159         if (cam->nbuffers) {
 160                 vfree(cam->frame[0].bufmem);
 161                 cam->nbuffers = 0;
 162         }
 163         cam->frame_current = NULL;
 164 }
 165
 166
 167 static void et61x251_empty_framequeues(struct et61x251_device* cam)
 168 {
 169         u32 i;
 170
 171         INIT_LIST_HEAD(&cam->inqueue);
 172         INIT_LIST_HEAD(&cam->outqueue);
 173
 174         for (i = 0; i < ET61X251_MAX_FRAMES; i++) {
 175                 cam->frame[i].state = F_UNUSED;
 176                 cam->frame[i].buf.bytesused = 0;
 177         }
 178 }
 179
 180
 181 static void et61x251_requeue_outqueue(struct et61x251_device* cam)
 182 {
 183         struct et61x251_frame_t *i;
 184
 185         list_for_each_entry(i, &cam->outqueue, frame) {
 186                 i->state = F_QUEUED;
 187                 list_add(&i->frame, &cam->inqueue);
 188         }
 189
 190         INIT_LIST_HEAD(&cam->outqueue);
 191 }
 192
 193
 194 static void et61x251_queue_unusedframes(struct et61x251_device* cam)
 195 {
 196         unsigned long lock_flags;
 197         u32 i;
 198
 199         for (i = 0; i < cam->nbuffers; i++)
 200                 if (cam->frame[i].state == F_UNUSED) {
 201                         cam->frame[i].state = F_QUEUED;
 202                         spin_lock_irqsave(&cam->queue_lock, lock_flags);
 203                         list_add_tail(&cam->frame[i].frame, &cam->inqueue);
 204                         spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
 205                 }
 206 }
 207
 208 /*****************************************************************************/
 209
 210 int et61x251_write_reg(struct et61x251_device* cam, u8 value, u16 index)
 211 {
 212         struct usb_device* udev = cam->usbdev;
 213         u8* buff = cam->control_buffer;
 214         int res;
 215
 216         *buff = value;
 217
 218         res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
 219                               0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
 220         if (res < 0) {
 221                 DBG(3, "Failed to write a register (value 0x%02X, index "
 222                        "0x%02X, error %d)", value, index, res);
 223                 return -1;
 224         }
 225
 226         return 0;
 227 }
 228
 229
 230 static int et61x251_read_reg(struct et61x251_device* cam, u16 index)
 231 {
 232         struct usb_device* udev = cam->usbdev;
 233         u8* buff = cam->control_buffer;
 234         int res;
 235
 236         res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
 237                               0, index, buff, 1, ET61X251_CTRL_TIMEOUT);
 238         if (res < 0)
 239                 DBG(3, "Failed to read a register (index 0x%02X, error %d)",
 240                     index, res);
 241
 242         return (res >= 0) ? (int)(*buff) : -1;
 243 }
 244
 245
 246 static int
 247 et61x251_i2c_wait(struct et61x251_device* cam,
 248                   const struct et61x251_sensor* sensor)
 249 {
 250         int i, r;
 251
 252         for (i = 1; i <= 8; i++) {
 253                 if (sensor->interface == ET61X251_I2C_3WIRES) {
 254                         r = et61x251_read_reg(cam, 0x8e);
 255                         if (!(r & 0x02) && (r >= 0))
 256                                 return 0;
 257                 } else {
 258                         r = et61x251_read_reg(cam, 0x8b);
 259                         if (!(r & 0x01) && (r >= 0))
 260                                 return 0;
 261                 }
 262                 if (r < 0)
 263                         return -EIO;
 264                 udelay(8*8); /* minimum for sensors at 400kHz */
 265         }
 266
 267         return -EBUSY;
 268 }
 269
 270
 271 int
 272 et61x251_i2c_raw_write(struct et61x251_device* cam, u8 n, u8 data1, u8 data2,
 273                        u8 data3, u8 data4, u8 data5, u8 data6, u8 data7,
 274                        u8 data8, u8 address)
 275 {
 276         struct usb_device* udev = cam->usbdev;
 277         u8* data = cam->control_buffer;
 278         int err = 0, res;
 279
 280         data[0] = data2;
 281         data[1] = data3;
 282         data[2] = data4;
 283         data[3] = data5;
 284         data[4] = data6;
 285         data[5] = data7;
 286         data[6] = data8;
 287         res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
 288                               0, 0x81, data, n-1, ET61X251_CTRL_TIMEOUT);
 289         if (res < 0)
 290                 err += res;
 291
 292         data[0] = address;
 293         data[1] = cam->sensor.i2c_slave_id;
 294         data[2] = cam->sensor.rsta | 0x02 | (n << 4);
 295         res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
 296                               0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
 297         if (res < 0)
 298                 err += res;
 299
 300         /* Start writing through the serial interface */
 301         data[0] = data1;
 302         res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
 303                               0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
 304         if (res < 0)
 305                 err += res;
 306
 307         err += et61x251_i2c_wait(cam, &cam->sensor);
 308
 309         if (err)
 310                 DBG(3, "I2C raw write failed for %s image sensor",
 311                     cam->sensor.name);
 312
 313         PDBGG("I2C raw write: %u bytes, address = 0x%02X, data1 = 0x%02X, "
 314               "data2 = 0x%02X, data3 = 0x%02X, data4 = 0x%02X, data5 = 0x%02X,"
 315               " data6 = 0x%02X, data7 = 0x%02X, data8 = 0x%02X", n, address,
 316               data1, data2, data3, data4, data5, data6, data7, data8);
 317
 318         return err ? -1 : 0;
 319
 320 }
 321
 322
 323 /*****************************************************************************/
 324
 325 static void et61x251_urb_complete(struct urb *urb)
 326 {
 327         struct et61x251_device* cam = urb->context;
 328         struct et61x251_frame_t** f;
 329         size_t imagesize;
 330         u8 i;
 331         int err = 0;
 332
 333         if (urb->status == -ENOENT)
 334                 return;
 335
 336         f = &cam->frame_current;
 337
 338         if (cam->stream == STREAM_INTERRUPT) {
 339                 cam->stream = STREAM_OFF;
 340                 if ((*f))
 341                         (*f)->state = F_QUEUED;
 342                 DBG(3, "Stream interrupted");
 343                 wake_up(&cam->wait_stream);
 344         }
 345
 346         if (cam->state & DEV_DISCONNECTED)
 347                 return;
 348
 349         if (cam->state & DEV_MISCONFIGURED) {
 350                 wake_up_interruptible(&cam->wait_frame);
 351                 return;
 352         }
 353
 354         if (cam->stream == STREAM_OFF || list_empty(&cam->inqueue))
 355                 goto resubmit_urb;
 356
 357         if (!(*f))
 358                 (*f) = list_entry(cam->inqueue.next, struct et61x251_frame_t,
 359                                   frame);
 360
 361         imagesize = (cam->sensor.pix_format.width *
 362                      cam->sensor.pix_format.height *
 363                      cam->sensor.pix_format.priv) / 8;
 364
 365         for (i = 0; i < urb->number_of_packets; i++) {
 366                 unsigned int len, status;
 367                 void *pos;
 368                 u8* b1, * b2, sof;
 369                 const u8 VOID_BYTES = 6;
 370                 size_t imglen;
 371
 372                 len = urb->iso_frame_desc[i].actual_length;
 373                 status = urb->iso_frame_desc[i].status;
 374                 pos = urb->iso_frame_desc[i].offset + urb->transfer_buffer;
 375
 376                 if (status) {
 377                         DBG(3, "Error in isochronous frame");
 378                         (*f)->state = F_ERROR;
 379                         continue;
 380                 }
 381
 382                 b1 = pos++;
 383                 b2 = pos++;
 384                 sof = ((*b1 & 0x3f) == 63);
 385                 imglen = ((*b1 & 0xc0) << 2) | *b2;
 386
 387                 PDBGG("Isochrnous frame: length %u, #%u i, image length %zu",
 388                       len, i, imglen);
 389
 390                 if ((*f)->state == F_QUEUED || (*f)->state == F_ERROR)
 391 start_of_frame:
 392                         if (sof) {
 393                                 (*f)->state = F_GRABBING;
 394                                 (*f)->buf.bytesused = 0;
 395                                 do_gettimeofday(&(*f)->buf.timestamp);
 396                                 pos += 22;
 397                                 DBG(3, "SOF detected: new video frame");
 398                         }
 399
 400                 if ((*f)->state == F_GRABBING) {
 401                         if (sof && (*f)->buf.bytesused) {
 402                                 if (cam->sensor.pix_format.pixelformat ==
 403                                                          V4L2_PIX_FMT_ET61X251)
 404                                         goto end_of_frame;
 405                                 else {
 406                                         DBG(3, "Not expected SOF detected "
 407                                                "after %lu bytes",
 408                                            (unsigned long)(*f)->buf.bytesused);
 409                                         (*f)->state = F_ERROR;
 410                                         continue;
 411                                 }
 412                         }
 413
 414                         if ((*f)->buf.bytesused + imglen > imagesize) {
 415                                 DBG(3, "Video frame size exceeded");
 416                                 (*f)->state = F_ERROR;
 417                                 continue;
 418                         }
 419
 420                         pos += VOID_BYTES;
 421
 422                         memcpy((*f)->bufmem+(*f)->buf.bytesused, pos, imglen);
 423                         (*f)->buf.bytesused += imglen;
 424
 425                         if ((*f)->buf.bytesused == imagesize) {
 426                                 u32 b;
 427 end_of_frame:
 428                                 b = (*f)->buf.bytesused;
 429                                 (*f)->state = F_DONE;
 430                                 (*f)->buf.sequence= ++cam->frame_count;
 431                                 spin_lock(&cam->queue_lock);
 432                                 list_move_tail(&(*f)->frame, &cam->outqueue);
 433                                 if (!list_empty(&cam->inqueue))
 434                                         (*f) = list_entry(cam->inqueue.next,
 435                                                        struct et61x251_frame_t,
 436                                                           frame);
 437                                 else
 438                                         (*f) = NULL;
 439                                 spin_unlock(&cam->queue_lock);
 440                                 DBG(3, "Video frame captured: : %lu bytes",
 441                                        (unsigned long)(b));
 442
 443                                 if (!(*f))
 444                                         goto resubmit_urb;
 445
 446                                 if (sof &&
 447                                     cam->sensor.pix_format.pixelformat ==
 448                                                          V4L2_PIX_FMT_ET61X251)
 449                                         goto start_of_frame;
 450                         }
 451                 }
 452         }
 453
 454 resubmit_urb:
 455         urb->dev = cam->usbdev;
 456         err = usb_submit_urb(urb, GFP_ATOMIC);
 457         if (err < 0 && err != -EPERM) {
 458                 cam->state |= DEV_MISCONFIGURED;
 459                 DBG(1, "usb_submit_urb() failed");
 460         }
 461
 462         wake_up_interruptible(&cam->wait_frame);
 463 }
 464
 465
 466 static int et61x251_start_transfer(struct et61x251_device* cam)
 467 {
 468         struct usb_device *udev = cam->usbdev;
 469         struct urb* urb;
 470         struct usb_host_interface* altsetting = usb_altnum_to_altsetting(
 471                                                    usb_ifnum_to_if(udev, 0),
 472                                                    ET61X251_ALTERNATE_SETTING);
 473         const unsigned int psz = le16_to_cpu(altsetting->
 474                                              endpoint[0].desc.wMaxPacketSize);
 475         s8 i, j;
 476         int err = 0;
 477
 478         for (i = 0; i < ET61X251_URBS; i++) {
 479                 cam->transfer_buffer[i] = kzalloc(ET61X251_ISO_PACKETS * psz,
 480                                                   GFP_KERNEL);
 481                 if (!cam->transfer_buffer[i]) {
 482                         err = -ENOMEM;
 483                         DBG(1, "Not enough memory");
 484                         goto free_buffers;
 485                 }
 486         }
 487
 488         for (i = 0; i < ET61X251_URBS; i++) {
 489                 urb = usb_alloc_urb(ET61X251_ISO_PACKETS, GFP_KERNEL);
 490                 cam->urb[i] = urb;
 491                 if (!urb) {
 492                         err = -ENOMEM;
 493                         DBG(1, "usb_alloc_urb() failed");
 494                         goto free_urbs;
 495                 }
 496                 urb->dev = udev;
 497                 urb->context = cam;
 498                 urb->pipe = usb_rcvisocpipe(udev, 1);
 499                 urb->transfer_flags = URB_ISO_ASAP;
 500                 urb->number_of_packets = ET61X251_ISO_PACKETS;
 501                 urb->complete = et61x251_urb_complete;
 502                 urb->transfer_buffer = cam->transfer_buffer[i];
 503                 urb->transfer_buffer_length = psz * ET61X251_ISO_PACKETS;
 504                 urb->interval = 1;
 505                 for (j = 0; j < ET61X251_ISO_PACKETS; j++) {
 506                         urb->iso_frame_desc[j].offset = psz * j;
 507                         urb->iso_frame_desc[j].length = psz;
 508                 }
 509         }
 510
 511         err = et61x251_write_reg(cam, 0x01, 0x03);
 512         err = et61x251_write_reg(cam, 0x00, 0x03);
 513         err = et61x251_write_reg(cam, 0x08, 0x03);
 514         if (err) {
 515                 err = -EIO;
 516                 DBG(1, "I/O hardware error");
 517                 goto free_urbs;
 518         }
 519
 520         err = usb_set_interface(udev, 0, ET61X251_ALTERNATE_SETTING);
 521         if (err) {
 522                 DBG(1, "usb_set_interface() failed");
 523                 goto free_urbs;
 524         }
 525
 526         cam->frame_current = NULL;
 527
 528         for (i = 0; i < ET61X251_URBS; i++) {
 529                 err = usb_submit_urb(cam->urb[i], GFP_KERNEL);
 530                 if (err) {
 531                         for (j = i-1; j >= 0; j--)
 532                                 usb_kill_urb(cam->urb[j]);
 533                         DBG(1, "usb_submit_urb() failed, error %d", err);
 534                         goto free_urbs;
 535                 }
 536         }
 537
 538         return 0;
 539
 540 free_urbs:
 541         for (i = 0; (i < ET61X251_URBS) && cam->urb[i]; i++)
 542                 usb_free_urb(cam->urb[i]);
 543
 544 free_buffers:
 545         for (i = 0; (i < ET61X251_URBS) && cam->transfer_buffer[i]; i++)
 546                 kfree(cam->transfer_buffer[i]);
 547
 548         return err;
 549 }
 550
 551
 552 static int et61x251_stop_transfer(struct et61x251_device* cam)
 553 {
 554         struct usb_device *udev = cam->usbdev;
 555         s8 i;
 556         int err = 0;
 557
 558         if (cam->state & DEV_DISCONNECTED)
 559                 return 0;
 560
 561         for (i = ET61X251_URBS-1; i >= 0; i--) {
 562                 usb_kill_urb(cam->urb[i]);
 563                 usb_free_urb(cam->urb[i]);
 564                 kfree(cam->transfer_buffer[i]);
 565         }
 566
 567         err = usb_set_interface(udev, 0, 0); /* 0 Mb/s */
 568         if (err)
 569                 DBG(3, "usb_set_interface() failed");
 570
 571         return err;
 572 }
 573
 574
 575 static int et61x251_stream_interrupt(struct et61x251_device* cam)
 576 {
 577         long timeout;
 578
 579         cam->stream = STREAM_INTERRUPT;
 580         timeout = wait_event_timeout(cam->wait_stream,
 581                                      (cam->stream == STREAM_OFF) ||
 582                                      (cam->state & DEV_DISCONNECTED),
 583                                      ET61X251_URB_TIMEOUT);
 584         if (cam->state & DEV_DISCONNECTED)
 585                 return -ENODEV;
 586         else if (cam->stream != STREAM_OFF) {
 587                 cam->state |= DEV_MISCONFIGURED;
 588                 DBG(1, "URB timeout reached. The camera is misconfigured. To "
 589                        "use it, close and open /dev/video%d again.",
 590                     cam->v4ldev->minor);
 591                 return -EIO;
 592         }
 593
 594         return 0;
 595 }
 596
 597 /*****************************************************************************/
 598
 599 #ifdef CONFIG_VIDEO_ADV_DEBUG
 600
 601 static int et61x251_i2c_try_read(struct et61x251_device* cam,
 602                                  const struct et61x251_sensor* sensor,
 603                                  u8 address)
 604 {
 605         struct usb_device* udev = cam->usbdev;
 606         u8* data = cam->control_buffer;
 607         int err = 0, res;
 608
 609         data[0] = address;
 610         data[1] = cam->sensor.i2c_slave_id;
 611         data[2] = cam->sensor.rsta | 0x10;
 612         data[3] = !(et61x251_read_reg(cam, 0x8b) & 0x02);
 613         res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
 614                               0, 0x88, data, 4, ET61X251_CTRL_TIMEOUT);
 615         if (res < 0)
 616                 err += res;
 617
 618         err += et61x251_i2c_wait(cam, sensor);
 619
 620         res = usb_control_msg(udev, usb_rcvctrlpipe(udev, 0), 0x00, 0xc1,
 621                               0, 0x80, data, 8, ET61X251_CTRL_TIMEOUT);
 622         if (res < 0)
 623                 err += res;
 624
 625         if (err)
 626                 DBG(3, "I2C read failed for %s image sensor", sensor->name);
 627
 628         PDBGG("I2C read: address 0x%02X, value: 0x%02X", address, data[0]);
 629
 630         return err ? -1 : (int)data[0];
 631 }
 632
 633
 634 static int et61x251_i2c_try_write(struct et61x251_device* cam,
 635                                   const struct et61x251_sensor* sensor,
 636                                   u8 address, u8 value)
 637 {
 638         struct usb_device* udev = cam->usbdev;
 639         u8* data = cam->control_buffer;
 640         int err = 0, res;
 641
 642         data[0] = address;
 643         data[1] = cam->sensor.i2c_slave_id;
 644         data[2] = cam->sensor.rsta | 0x12;
 645         res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
 646                               0, 0x88, data, 3, ET61X251_CTRL_TIMEOUT);
 647         if (res < 0)
 648                 err += res;
 649
 650         data[0] = value;
 651         res = usb_control_msg(udev, usb_sndctrlpipe(udev, 0), 0x00, 0x41,
 652                               0, 0x80, data, 1, ET61X251_CTRL_TIMEOUT);
 653         if (res < 0)
 654                 err += res;
 655
 656         err += et61x251_i2c_wait(cam, sensor);
 657
 658         if (err)
 659                 DBG(3, "I2C write failed for %s image sensor", sensor->name);
 660
 661         PDBGG("I2C write: address 0x%02X, value: 0x%02X", address, value);
 662
 663         return err ? -1 : 0;
 664 }
 665
 666 static int et61x251_i2c_read(struct et61x251_device* cam, u8 address)
 667 {
 668         return et61x251_i2c_try_read(cam, &cam->sensor, address);
 669 }
 670
 671 static int et61x251_i2c_write(struct et61x251_device* cam,
 672                               u8 address, u8 value)
 673 {
 674         return et61x251_i2c_try_write(cam, &cam->sensor, address, value);
 675 }
 676
 677 static u8 et61x251_strtou8(const char* buff, size_t len, ssize_t* count)
 678 {
 679         char str[5];
 680         char* endp;
 681         unsigned long val;
 682
 683         if (len < 4) {
 684                 strncpy(str, buff, len);
 685                 str[len] = '\0';
 686         } else {
 687                 strncpy(str, buff, 4);
 688                 str[4] = '\0';
 689         }
 690
 691         val = simple_strtoul(str, &endp, 0);
 692
 693         *count = 0;
 694         if (val <= 0xff)
 695                 *count = (ssize_t)(endp - str);
 696         if ((*count) && (len == *count+1) && (buff[*count] == '\n'))
 697                 *count += 1;
 698
 699         return (u8)val;
 700 }
 701
 702 /*
 703    NOTE 1: being inside one of the following methods implies that the v4l
 704            device exists for sure (see kobjects and reference counters)
 705    NOTE 2: buffers are PAGE_SIZE long
 706 */
 707
 708 static ssize_t et61x251_show_reg(struct device* cd,
 709                                  struct device_attribute *attr, char* buf)
 710 {
 711         struct et61x251_device* cam;
 712         ssize_t count;
 713
 714         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 715                 return -ERESTARTSYS;
 716
 717         cam = video_get_drvdata(to_video_device(cd));
 718         if (!cam) {
 719                 mutex_unlock(&et61x251_sysfs_lock);
 720                 return -ENODEV;
 721         }
 722
 723         count = sprintf(buf, "%u\n", cam->sysfs.reg);
 724
 725         mutex_unlock(&et61x251_sysfs_lock);
 726
 727         return count;
 728 }
 729
 730
 731 static ssize_t
 732 et61x251_store_reg(struct device* cd,
 733                    struct device_attribute *attr, const char* buf, size_t len)
 734 {
 735         struct et61x251_device* cam;
 736         u8 index;
 737         ssize_t count;
 738
 739         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 740                 return -ERESTARTSYS;
 741
 742         cam = video_get_drvdata(to_video_device(cd));
 743         if (!cam) {
 744                 mutex_unlock(&et61x251_sysfs_lock);
 745                 return -ENODEV;
 746         }
 747
 748         index = et61x251_strtou8(buf, len, &count);
 749         if (index > 0x8e || !count) {
 750                 mutex_unlock(&et61x251_sysfs_lock);
 751                 return -EINVAL;
 752         }
 753
 754         cam->sysfs.reg = index;
 755
 756         DBG(2, "Moved ET61X[12]51 register index to 0x%02X", cam->sysfs.reg);
 757         DBG(3, "Written bytes: %zd", count);
 758
 759         mutex_unlock(&et61x251_sysfs_lock);
 760
 761         return count;
 762 }
 763
 764
 765 static ssize_t et61x251_show_val(struct device* cd,
 766                                  struct device_attribute *attr, char* buf)
 767 {
 768         struct et61x251_device* cam;
 769         ssize_t count;
 770         int val;
 771
 772         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 773                 return -ERESTARTSYS;
 774
 775         cam = video_get_drvdata(to_video_device(cd));
 776         if (!cam) {
 777                 mutex_unlock(&et61x251_sysfs_lock);
 778                 return -ENODEV;
 779         }
 780
 781         if ((val = et61x251_read_reg(cam, cam->sysfs.reg)) < 0) {
 782                 mutex_unlock(&et61x251_sysfs_lock);
 783                 return -EIO;
 784         }
 785
 786         count = sprintf(buf, "%d\n", val);
 787
 788         DBG(3, "Read bytes: %zd", count);
 789
 790         mutex_unlock(&et61x251_sysfs_lock);
 791
 792         return count;
 793 }
 794
 795
 796 static ssize_t
 797 et61x251_store_val(struct device* cd, struct device_attribute *attr,
 798                    const char* buf, size_t len)
 799 {
 800         struct et61x251_device* cam;
 801         u8 value;
 802         ssize_t count;
 803         int err;
 804
 805         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 806                 return -ERESTARTSYS;
 807
 808         cam = video_get_drvdata(to_video_device(cd));
 809         if (!cam) {
 810                 mutex_unlock(&et61x251_sysfs_lock);
 811                 return -ENODEV;
 812         }
 813
 814         value = et61x251_strtou8(buf, len, &count);
 815         if (!count) {
 816                 mutex_unlock(&et61x251_sysfs_lock);
 817                 return -EINVAL;
 818         }
 819
 820         err = et61x251_write_reg(cam, value, cam->sysfs.reg);
 821         if (err) {
 822                 mutex_unlock(&et61x251_sysfs_lock);
 823                 return -EIO;
 824         }
 825
 826         DBG(2, "Written ET61X[12]51 reg. 0x%02X, val. 0x%02X",
 827             cam->sysfs.reg, value);
 828         DBG(3, "Written bytes: %zd", count);
 829
 830         mutex_unlock(&et61x251_sysfs_lock);
 831
 832         return count;
 833 }
 834
 835
 836 static ssize_t et61x251_show_i2c_reg(struct device* cd,
 837                                      struct device_attribute *attr, char* buf)
 838 {
 839         struct et61x251_device* cam;
 840         ssize_t count;
 841
 842         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 843                 return -ERESTARTSYS;
 844
 845         cam = video_get_drvdata(to_video_device(cd));
 846         if (!cam) {
 847                 mutex_unlock(&et61x251_sysfs_lock);
 848                 return -ENODEV;
 849         }
 850
 851         count = sprintf(buf, "%u\n", cam->sysfs.i2c_reg);
 852
 853         DBG(3, "Read bytes: %zd", count);
 854
 855         mutex_unlock(&et61x251_sysfs_lock);
 856
 857         return count;
 858 }
 859
 860
 861 static ssize_t
 862 et61x251_store_i2c_reg(struct device* cd, struct device_attribute *attr,
 863                        const char* buf, size_t len)
 864 {
 865         struct et61x251_device* cam;
 866         u8 index;
 867         ssize_t count;
 868
 869         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 870                 return -ERESTARTSYS;
 871
 872         cam = video_get_drvdata(to_video_device(cd));
 873         if (!cam) {
 874                 mutex_unlock(&et61x251_sysfs_lock);
 875                 return -ENODEV;
 876         }
 877
 878         index = et61x251_strtou8(buf, len, &count);
 879         if (!count) {
 880                 mutex_unlock(&et61x251_sysfs_lock);
 881                 return -EINVAL;
 882         }
 883
 884         cam->sysfs.i2c_reg = index;
 885
 886         DBG(2, "Moved sensor register index to 0x%02X", cam->sysfs.i2c_reg);
 887         DBG(3, "Written bytes: %zd", count);
 888
 889         mutex_unlock(&et61x251_sysfs_lock);
 890
 891         return count;
 892 }
 893
 894
 895 static ssize_t et61x251_show_i2c_val(struct device* cd,
 896                                      struct device_attribute *attr, char* buf)
 897 {
 898         struct et61x251_device* cam;
 899         ssize_t count;
 900         int val;
 901
 902         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 903                 return -ERESTARTSYS;
 904
 905         cam = video_get_drvdata(to_video_device(cd));
 906         if (!cam) {
 907                 mutex_unlock(&et61x251_sysfs_lock);
 908                 return -ENODEV;
 909         }
 910
 911         if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
 912                 mutex_unlock(&et61x251_sysfs_lock);
 913                 return -ENOSYS;
 914         }
 915
 916         if ((val = et61x251_i2c_read(cam, cam->sysfs.i2c_reg)) < 0) {
 917                 mutex_unlock(&et61x251_sysfs_lock);
 918                 return -EIO;
 919         }
 920
 921         count = sprintf(buf, "%d\n", val);
 922
 923         DBG(3, "Read bytes: %zd", count);
 924
 925         mutex_unlock(&et61x251_sysfs_lock);
 926
 927         return count;
 928 }
 929
 930
 931 static ssize_t
 932 et61x251_store_i2c_val(struct device* cd, struct device_attribute *attr,
 933                        const char* buf, size_t len)
 934 {
 935         struct et61x251_device* cam;
 936         u8 value;
 937         ssize_t count;
 938         int err;
 939
 940         if (mutex_lock_interruptible(&et61x251_sysfs_lock))
 941                 return -ERESTARTSYS;
 942
 943         cam = video_get_drvdata(to_video_device(cd));
 944         if (!cam) {
 945                 mutex_unlock(&et61x251_sysfs_lock);
 946                 return -ENODEV;
 947         }
 948
 949         if (!(cam->sensor.sysfs_ops & ET61X251_I2C_READ)) {
 950                 mutex_unlock(&et61x251_sysfs_lock);
 951                 return -ENOSYS;
 952         }
 953
 954         value = et61x251_strtou8(buf, len, &count);
 955         if (!count) {
 956                 mutex_unlock(&et61x251_sysfs_lock);
 957                 return -EINVAL;
 958         }
 959
 960         err = et61x251_i2c_write(cam, cam->sysfs.i2c_reg, value);
 961         if (err) {
 962                 mutex_unlock(&et61x251_sysfs_lock);
 963                 return -EIO;
 964         }
 965
 966         DBG(2, "Written sensor reg. 0x%02X, val. 0x%02X",
 967             cam->sysfs.i2c_reg, value);
 968         DBG(3, "Written bytes: %zd", count);
 969
 970         mutex_unlock(&et61x251_sysfs_lock);
 971
 972         return count;
 973 }
 974
 975
 976 static DEVICE_ATTR(reg, S_IRUGO | S_IWUSR,
 977                    et61x251_show_reg, et61x251_store_reg);
 978 static DEVICE_ATTR(val, S_IRUGO | S_IWUSR,
 979                    et61x251_show_val, et61x251_store_val);
 980 static DEVICE_ATTR(i2c_reg, S_IRUGO | S_IWUSR,
 981                    et61x251_show_i2c_reg, et61x251_store_i2c_reg);
 982 static DEVICE_ATTR(i2c_val, S_IRUGO | S_IWUSR,
 983                    et61x251_show_i2c_val, et61x251_store_i2c_val);
 984
 985
 986 static int et61x251_create_sysfs(struct et61x251_device* cam)
 987 {
 988         struct device *classdev = &(cam->v4ldev->class_dev);
 989         int err = 0;
 990
 991         if ((err = device_create_file(classdev, &dev_attr_reg)))
 992                 goto err_out;
 993         if ((err = device_create_file(classdev, &dev_attr_val)))
 994                 goto err_reg;
 995
 996         if (cam->sensor.sysfs_ops) {
 997                 if ((err = device_create_file(classdev, &dev_attr_i2c_reg)))
 998                         goto err_val;
 999                 if ((err = device_create_file(classdev, &dev_attr_i2c_val)))
1000                         goto err_i2c_reg;
1001         }
1002
1003 err_i2c_reg:
1004         if (cam->sensor.sysfs_ops)
1005                 device_remove_file(classdev, &dev_attr_i2c_reg);
1006 err_val:
1007         device_remove_file(classdev, &dev_attr_val);
1008 err_reg:
1009         device_remove_file(classdev, &dev_attr_reg);
1010 err_out:
1011         return err;
1012 }
1013 #endif /* CONFIG_VIDEO_ADV_DEBUG */
1014
1015 /*****************************************************************************/
1016
1017 static int
1018 et61x251_set_pix_format(struct et61x251_device* cam,
1019                         struct v4l2_pix_format* pix)
1020 {
1021         int r, err = 0;
1022
1023         if ((r = et61x251_read_reg(cam, 0x12)) < 0)
1024                 err += r;
1025         if (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
1026                 err += et61x251_write_reg(cam, r & 0xfd, 0x12);
1027         else
1028                 err += et61x251_write_reg(cam, r | 0x02, 0x12);
1029
1030         return err ? -EIO : 0;
1031 }
1032
1033
1034 static int
1035 et61x251_set_compression(struct et61x251_device* cam,
1036                          struct v4l2_jpegcompression* compression)
1037 {
1038         int r, err = 0;
1039
1040         if ((r = et61x251_read_reg(cam, 0x12)) < 0)
1041                 err += r;
1042         if (compression->quality == 0)
1043                 err += et61x251_write_reg(cam, r & 0xfb, 0x12);
1044         else
1045                 err += et61x251_write_reg(cam, r | 0x04, 0x12);
1046
1047         return err ? -EIO : 0;
1048 }
1049
1050
1051 static int et61x251_set_scale(struct et61x251_device* cam, u8 scale)
1052 {
1053         int r = 0, err = 0;
1054
1055         r = et61x251_read_reg(cam, 0x12);
1056         if (r < 0)
1057                 err += r;
1058
1059         if (scale == 1)
1060                 err += et61x251_write_reg(cam, r & ~0x01, 0x12);
1061         else if (scale == 2)
1062                 err += et61x251_write_reg(cam, r | 0x01, 0x12);
1063
1064         if (err)
1065                 return -EIO;
1066
1067         PDBGG("Scaling factor: %u", scale);
1068
1069         return 0;
1070 }
1071
1072
1073 static int
1074 et61x251_set_crop(struct et61x251_device* cam, struct v4l2_rect* rect)
1075 {
1076         struct et61x251_sensor* s = &cam->sensor;
1077         u16 fmw_sx = (u16)(rect->left - s->cropcap.bounds.left +
1078                            s->active_pixel.left),
1079             fmw_sy = (u16)(rect->top - s->cropcap.bounds.top +
1080                            s->active_pixel.top),
1081             fmw_length = (u16)(rect->width),
1082             fmw_height = (u16)(rect->height);
1083         int err = 0;
1084
1085         err += et61x251_write_reg(cam, fmw_sx & 0xff, 0x69);
1086         err += et61x251_write_reg(cam, fmw_sy & 0xff, 0x6a);
1087         err += et61x251_write_reg(cam, fmw_length & 0xff, 0x6b);
1088         err += et61x251_write_reg(cam, fmw_height & 0xff, 0x6c);
1089         err += et61x251_write_reg(cam, (fmw_sx >> 8) | ((fmw_sy & 0x300) >> 6)
1090                                        | ((fmw_length & 0x300) >> 4)
1091                                        | ((fmw_height & 0x300) >> 2), 0x6d);
1092         if (err)
1093                 return -EIO;
1094
1095         PDBGG("fmw_sx, fmw_sy, fmw_length, fmw_height: %u %u %u %u",
1096               fmw_sx, fmw_sy, fmw_length, fmw_height);
1097
1098         return 0;
1099 }
1100
1101
1102 static int et61x251_init(struct et61x251_device* cam)
1103 {
1104         struct et61x251_sensor* s = &cam->sensor;
1105         struct v4l2_control ctrl;
1106         struct v4l2_queryctrl *qctrl;
1107         struct v4l2_rect* rect;
1108         u8 i = 0;
1109         int err = 0;
1110
1111         if (!(cam->state & DEV_INITIALIZED)) {
1112                 mutex_init(&cam->open_mutex);
1113                 init_waitqueue_head(&cam->wait_open);
1114                 qctrl = s->qctrl;
1115                 rect = &(s->cropcap.defrect);
1116                 cam->compression.quality = ET61X251_COMPRESSION_QUALITY;
1117         } else { /* use current values */
1118                 qctrl = s->_qctrl;
1119                 rect = &(s->_rect);
1120         }
1121
1122         err += et61x251_set_scale(cam, rect->width / s->pix_format.width);
1123         err += et61x251_set_crop(cam, rect);
1124         if (err)
1125                 return err;
1126
1127         if (s->init) {
1128                 err = s->init(cam);
1129                 if (err) {
1130                         DBG(3, "Sensor initialization failed");
1131                         return err;
1132                 }
1133         }
1134
1135         err += et61x251_set_compression(cam, &cam->compression);
1136         err += et61x251_set_pix_format(cam, &s->pix_format);
1137         if (s->set_pix_format)
1138                 err += s->set_pix_format(cam, &s->pix_format);
1139         if (err)
1140                 return err;
1141
1142         if (s->pix_format.pixelformat == V4L2_PIX_FMT_ET61X251)
1143                 DBG(3, "Compressed video format is active, quality %d",
1144                     cam->compression.quality);
1145         else
1146                 DBG(3, "Uncompressed video format is active");
1147
1148         if (s->set_crop)
1149                 if ((err = s->set_crop(cam, rect))) {
1150                         DBG(3, "set_crop() failed");
1151                         return err;
1152                 }
1153
1154         if (s->set_ctrl) {
1155                 for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
1156                         if (s->qctrl[i].id != 0 &&
1157                             !(s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)) {
1158                                 ctrl.id = s->qctrl[i].id;
1159                                 ctrl.value = qctrl[i].default_value;
1160                                 err = s->set_ctrl(cam, &ctrl);
1161                                 if (err) {
1162                                         DBG(3, "Set %s control failed",
1163                                             s->qctrl[i].name);
1164                                         return err;
1165                                 }
1166                                 DBG(3, "Image sensor supports '%s' control",
1167                                     s->qctrl[i].name);
1168                         }
1169         }
1170
1171         if (!(cam->state & DEV_INITIALIZED)) {
1172                 mutex_init(&cam->fileop_mutex);
1173                 spin_lock_init(&cam->queue_lock);
1174                 init_waitqueue_head(&cam->wait_frame);
1175                 init_waitqueue_head(&cam->wait_stream);
1176                 cam->nreadbuffers = 2;
1177                 memcpy(s->_qctrl, s->qctrl, sizeof(s->qctrl));
1178                 memcpy(&(s->_rect), &(s->cropcap.defrect),
1179                        sizeof(struct v4l2_rect));
1180                 cam->state |= DEV_INITIALIZED;
1181         }
1182
1183         DBG(2, "Initialization succeeded");
1184         return 0;
1185 }
1186
1187 /*****************************************************************************/
1188
1189 static void et61x251_release_resources(struct kref *kref)
1190 {
1191         struct et61x251_device *cam;
1192
1193         mutex_lock(&et61x251_sysfs_lock);
1194
1195         cam = container_of(kref, struct et61x251_device, kref);
1196
1197         DBG(2, "V4L2 device /dev/video%d deregistered", cam->v4ldev->minor);
1198         video_set_drvdata(cam->v4ldev, NULL);
1199         video_unregister_device(cam->v4ldev);
1200         usb_put_dev(cam->usbdev);
1201         kfree(cam->control_buffer);
1202         kfree(cam);
1203
1204         mutex_unlock(&et61x251_sysfs_lock);
1205 }
1206
1207
1208 static int et61x251_open(struct inode* inode, struct file* filp)
1209 {
1210         struct et61x251_device* cam;
1211         int err = 0;
1212
1213         if (!down_read_trylock(&et61x251_dev_lock))
1214                 return -ERESTARTSYS;
1215
1216         cam = video_get_drvdata(video_devdata(filp));
1217
1218         if (wait_for_completion_interruptible(&cam->probe)) {
1219                 up_read(&et61x251_dev_lock);
1220                 return -ERESTARTSYS;
1221         }
1222
1223         kref_get(&cam->kref);
1224
1225         if (mutex_lock_interruptible(&cam->open_mutex)) {
1226                 kref_put(&cam->kref, et61x251_release_resources);
1227                 up_read(&et61x251_dev_lock);
1228                 return -ERESTARTSYS;
1229         }
1230
1231         if (cam->state & DEV_DISCONNECTED) {
1232                 DBG(1, "Device not present");
1233                 err = -ENODEV;
1234                 goto out;
1235         }
1236
1237         if (cam->users) {
1238                 DBG(2, "Device /dev/video%d is already in use",
1239                        cam->v4ldev->minor);
1240                 DBG(3, "Simultaneous opens are not supported");
1241                 if ((filp->f_flags & O_NONBLOCK) ||
1242                     (filp->f_flags & O_NDELAY)) {
1243                         err = -EWOULDBLOCK;
1244                         goto out;
1245                 }
1246                 DBG(2, "A blocking open() has been requested. Wait for the "
1247                        "device to be released...");
1248                 up_read(&et61x251_dev_lock);
1249                 err = wait_event_interruptible_exclusive(cam->wait_open,
1250                                                 (cam->state & DEV_DISCONNECTED)
1251                                                          || !cam->users);
1252                 down_read(&et61x251_dev_lock);
1253                 if (err)
1254                         goto out;
1255                 if (cam->state & DEV_DISCONNECTED) {
1256                         err = -ENODEV;
1257                         goto out;
1258                 }
1259         }
1260
1261         if (cam->state & DEV_MISCONFIGURED) {
1262                 err = et61x251_init(cam);
1263                 if (err) {
1264                         DBG(1, "Initialization failed again. "
1265                                "I will retry on next open().");
1266                         goto out;
1267                 }
1268                 cam->state &= ~DEV_MISCONFIGURED;
1269         }
1270
1271         if ((err = et61x251_start_transfer(cam)))
1272                 goto out;
1273
1274         filp->private_data = cam;
1275         cam->users++;
1276         cam->io = IO_NONE;
1277         cam->stream = STREAM_OFF;
1278         cam->nbuffers = 0;
1279         cam->frame_count = 0;
1280         et61x251_empty_framequeues(cam);
1281
1282         DBG(3, "Video device /dev/video%d is open", cam->v4ldev->minor);
1283
1284 out:
1285         mutex_unlock(&cam->open_mutex);
1286         if (err)
1287                 kref_put(&cam->kref, et61x251_release_resources);
1288         up_read(&et61x251_dev_lock);
1289         return err;
1290 }
1291
1292
1293 static int et61x251_release(struct inode* inode, struct file* filp)
1294 {
1295         struct et61x251_device* cam;
1296
1297         down_write(&et61x251_dev_lock);
1298
1299         cam = video_get_drvdata(video_devdata(filp));
1300
1301         et61x251_stop_transfer(cam);
1302         et61x251_release_buffers(cam);
1303         cam->users--;
1304         wake_up_interruptible_nr(&cam->wait_open, 1);
1305
1306         DBG(3, "Video device /dev/video%d closed", cam->v4ldev->minor);
1307
1308         kref_put(&cam->kref, et61x251_release_resources);
1309
1310         up_write(&et61x251_dev_lock);
1311
1312         return 0;
1313 }
1314
1315
1316 static ssize_t
1317 et61x251_read(struct file* filp, char __user * buf,
1318               size_t count, loff_t* f_pos)
1319 {
1320         struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
1321         struct et61x251_frame_t* f, * i;
1322         unsigned long lock_flags;
1323         long timeout;
1324         int err = 0;
1325
1326         if (mutex_lock_interruptible(&cam->fileop_mutex))
1327                 return -ERESTARTSYS;
1328
1329         if (cam->state & DEV_DISCONNECTED) {
1330                 DBG(1, "Device not present");
1331                 mutex_unlock(&cam->fileop_mutex);
1332                 return -ENODEV;
1333         }
1334
1335         if (cam->state & DEV_MISCONFIGURED) {
1336                 DBG(1, "The camera is misconfigured. Close and open it "
1337                        "again.");
1338                 mutex_unlock(&cam->fileop_mutex);
1339                 return -EIO;
1340         }
1341
1342         if (cam->io == IO_MMAP) {
1343                 DBG(3, "Close and open the device again to choose the read "
1344                        "method");
1345                 mutex_unlock(&cam->fileop_mutex);
1346                 return -EBUSY;
1347         }
1348
1349         if (cam->io == IO_NONE) {
1350                 if (!et61x251_request_buffers(cam, cam->nreadbuffers,
1351                                               IO_READ)) {
1352                         DBG(1, "read() failed, not enough memory");
1353                         mutex_unlock(&cam->fileop_mutex);
1354                         return -ENOMEM;
1355                 }
1356                 cam->io = IO_READ;
1357                 cam->stream = STREAM_ON;
1358         }
1359
1360         if (list_empty(&cam->inqueue)) {
1361                 if (!list_empty(&cam->outqueue))
1362                         et61x251_empty_framequeues(cam);
1363                 et61x251_queue_unusedframes(cam);
1364         }
1365
1366         if (!count) {
1367                 mutex_unlock(&cam->fileop_mutex);
1368                 return 0;
1369         }
1370
1371         if (list_empty(&cam->outqueue)) {
1372                 if (filp->f_flags & O_NONBLOCK) {
1373                         mutex_unlock(&cam->fileop_mutex);
1374                         return -EAGAIN;
1375                 }
1376                 timeout = wait_event_interruptible_timeout
1377                           ( cam->wait_frame,
1378                             (!list_empty(&cam->outqueue)) ||
1379                             (cam->state & DEV_DISCONNECTED) ||
1380                             (cam->state & DEV_MISCONFIGURED),
1381                             cam->module_param.frame_timeout *
1382                             1000 * msecs_to_jiffies(1) );
1383                 if (timeout < 0) {
1384                         mutex_unlock(&cam->fileop_mutex);
1385                         return timeout;
1386                 }
1387                 if (cam->state & DEV_DISCONNECTED) {
1388                         mutex_unlock(&cam->fileop_mutex);
1389                         return -ENODEV;
1390                 }
1391                 if (!timeout || (cam->state & DEV_MISCONFIGURED)) {
1392                         mutex_unlock(&cam->fileop_mutex);
1393                         return -EIO;
1394                 }
1395         }
1396
1397         f = list_entry(cam->outqueue.prev, struct et61x251_frame_t, frame);
1398
1399         if (count > f->buf.bytesused)
1400                 count = f->buf.bytesused;
1401
1402         if (copy_to_user(buf, f->bufmem, count)) {
1403                 err = -EFAULT;
1404                 goto exit;
1405         }
1406         *f_pos += count;
1407
1408 exit:
1409         spin_lock_irqsave(&cam->queue_lock, lock_flags);
1410         list_for_each_entry(i, &cam->outqueue, frame)
1411                 i->state = F_UNUSED;
1412         INIT_LIST_HEAD(&cam->outqueue);
1413         spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
1414
1415         et61x251_queue_unusedframes(cam);
1416
1417         PDBGG("Frame #%lu, bytes read: %zu",
1418               (unsigned long)f->buf.index, count);
1419
1420         mutex_unlock(&cam->fileop_mutex);
1421
1422         return err ? err : count;
1423 }
1424
1425
1426 static unsigned int et61x251_poll(struct file *filp, poll_table *wait)
1427 {
1428         struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
1429         struct et61x251_frame_t* f;
1430         unsigned long lock_flags;
1431         unsigned int mask = 0;
1432
1433         if (mutex_lock_interruptible(&cam->fileop_mutex))
1434                 return POLLERR;
1435
1436         if (cam->state & DEV_DISCONNECTED) {
1437                 DBG(1, "Device not present");
1438                 goto error;
1439         }
1440
1441         if (cam->state & DEV_MISCONFIGURED) {
1442                 DBG(1, "The camera is misconfigured. Close and open it "
1443                        "again.");
1444                 goto error;
1445         }
1446
1447         if (cam->io == IO_NONE) {
1448                 if (!et61x251_request_buffers(cam, cam->nreadbuffers,
1449                                               IO_READ)) {
1450                         DBG(1, "poll() failed, not enough memory");
1451                         goto error;
1452                 }
1453                 cam->io = IO_READ;
1454                 cam->stream = STREAM_ON;
1455         }
1456
1457         if (cam->io == IO_READ) {
1458                 spin_lock_irqsave(&cam->queue_lock, lock_flags);
1459                 list_for_each_entry(f, &cam->outqueue, frame)
1460                         f->state = F_UNUSED;
1461                 INIT_LIST_HEAD(&cam->outqueue);
1462                 spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
1463                 et61x251_queue_unusedframes(cam);
1464         }
1465
1466         poll_wait(filp, &cam->wait_frame, wait);
1467
1468         if (!list_empty(&cam->outqueue))
1469                 mask |= POLLIN | POLLRDNORM;
1470
1471         mutex_unlock(&cam->fileop_mutex);
1472
1473         return mask;
1474
1475 error:
1476         mutex_unlock(&cam->fileop_mutex);
1477         return POLLERR;
1478 }
1479
1480
1481 static void et61x251_vm_open(struct vm_area_struct* vma)
1482 {
1483         struct et61x251_frame_t* f = vma->vm_private_data;
1484         f->vma_use_count++;
1485 }
1486
1487
1488 static void et61x251_vm_close(struct vm_area_struct* vma)
1489 {
1490         /* NOTE: buffers are not freed here */
1491         struct et61x251_frame_t* f = vma->vm_private_data;
1492         f->vma_use_count--;
1493 }
1494
1495
1496 static struct vm_operations_struct et61x251_vm_ops = {
1497         .open = et61x251_vm_open,
1498         .close = et61x251_vm_close,
1499 };
1500
1501
1502 static int et61x251_mmap(struct file* filp, struct vm_area_struct *vma)
1503 {
1504         struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
1505         unsigned long size = vma->vm_end - vma->vm_start,
1506                       start = vma->vm_start;
1507         void *pos;
1508         u32 i;
1509
1510         if (mutex_lock_interruptible(&cam->fileop_mutex))
1511                 return -ERESTARTSYS;
1512
1513         if (cam->state & DEV_DISCONNECTED) {
1514                 DBG(1, "Device not present");
1515                 mutex_unlock(&cam->fileop_mutex);
1516                 return -ENODEV;
1517         }
1518
1519         if (cam->state & DEV_MISCONFIGURED) {
1520                 DBG(1, "The camera is misconfigured. Close and open it "
1521                        "again.");
1522                 mutex_unlock(&cam->fileop_mutex);
1523                 return -EIO;
1524         }
1525
1526         if (!(vma->vm_flags & (VM_WRITE | VM_READ))) {
1527                 mutex_unlock(&cam->fileop_mutex);
1528                 return -EACCES;
1529         }
1530
1531         if (cam->io != IO_MMAP ||
1532             size != PAGE_ALIGN(cam->frame[0].buf.length)) {
1533                 mutex_unlock(&cam->fileop_mutex);
1534                 return -EINVAL;
1535         }
1536
1537         for (i = 0; i < cam->nbuffers; i++) {
1538                 if ((cam->frame[i].buf.m.offset>>PAGE_SHIFT) == vma->vm_pgoff)
1539                         break;
1540         }
1541         if (i == cam->nbuffers) {
1542                 mutex_unlock(&cam->fileop_mutex);
1543                 return -EINVAL;
1544         }
1545
1546         vma->vm_flags |= VM_IO;
1547         vma->vm_flags |= VM_RESERVED;
1548
1549         pos = cam->frame[i].bufmem;
1550         while (size > 0) { /* size is page-aligned */
1551                 if (vm_insert_page(vma, start, vmalloc_to_page(pos))) {
1552                         mutex_unlock(&cam->fileop_mutex);
1553                         return -EAGAIN;
1554                 }
1555                 start += PAGE_SIZE;
1556                 pos += PAGE_SIZE;
1557                 size -= PAGE_SIZE;
1558         }
1559
1560         vma->vm_ops = &et61x251_vm_ops;
1561         vma->vm_private_data = &cam->frame[i];
1562         et61x251_vm_open(vma);
1563
1564         mutex_unlock(&cam->fileop_mutex);
1565
1566         return 0;
1567 }
1568
1569 /*****************************************************************************/
1570
1571 static int
1572 et61x251_vidioc_querycap(struct et61x251_device* cam, void __user * arg)
1573 {
1574         struct v4l2_capability cap = {
1575                 .driver = "et61x251",
1576                 .version = ET61X251_MODULE_VERSION_CODE,
1577                 .capabilities = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_READWRITE |
1578                                 V4L2_CAP_STREAMING,
1579         };
1580
1581         strlcpy(cap.card, cam->v4ldev->name, sizeof(cap.card));
1582         if (usb_make_path(cam->usbdev, cap.bus_info, sizeof(cap.bus_info)) < 0)
1583                 strlcpy(cap.bus_info, cam->usbdev->dev.bus_id,
1584                         sizeof(cap.bus_info));
1585
1586         if (copy_to_user(arg, &cap, sizeof(cap)))
1587                 return -EFAULT;
1588
1589         return 0;
1590 }
1591
1592
1593 static int
1594 et61x251_vidioc_enuminput(struct et61x251_device* cam, void __user * arg)
1595 {
1596         struct v4l2_input i;
1597
1598         if (copy_from_user(&i, arg, sizeof(i)))
1599                 return -EFAULT;
1600
1601         if (i.index)
1602                 return -EINVAL;
1603
1604         memset(&i, 0, sizeof(i));
1605         strcpy(i.name, "Camera");
1606         i.type = V4L2_INPUT_TYPE_CAMERA;
1607
1608         if (copy_to_user(arg, &i, sizeof(i)))
1609                 return -EFAULT;
1610
1611         return 0;
1612 }
1613
1614
1615 static int
1616 et61x251_vidioc_g_input(struct et61x251_device* cam, void __user * arg)
1617 {
1618         int index = 0;
1619
1620         if (copy_to_user(arg, &index, sizeof(index)))
1621                 return -EFAULT;
1622
1623         return 0;
1624 }
1625
1626
1627 static int
1628 et61x251_vidioc_s_input(struct et61x251_device* cam, void __user * arg)
1629 {
1630         int index;
1631
1632         if (copy_from_user(&index, arg, sizeof(index)))
1633                 return -EFAULT;
1634
1635         if (index != 0)
1636                 return -EINVAL;
1637
1638         return 0;
1639 }
1640
1641
1642 static int
1643 et61x251_vidioc_query_ctrl(struct et61x251_device* cam, void __user * arg)
1644 {
1645         struct et61x251_sensor* s = &cam->sensor;
1646         struct v4l2_queryctrl qc;
1647         u8 i;
1648
1649         if (copy_from_user(&qc, arg, sizeof(qc)))
1650                 return -EFAULT;
1651
1652         for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
1653                 if (qc.id && qc.id == s->qctrl[i].id) {
1654                         memcpy(&qc, &(s->qctrl[i]), sizeof(qc));
1655                         if (copy_to_user(arg, &qc, sizeof(qc)))
1656                                 return -EFAULT;
1657                         return 0;
1658                 }
1659
1660         return -EINVAL;
1661 }
1662
1663
1664 static int
1665 et61x251_vidioc_g_ctrl(struct et61x251_device* cam, void __user * arg)
1666 {
1667         struct et61x251_sensor* s = &cam->sensor;
1668         struct v4l2_control ctrl;
1669         int err = 0;
1670         u8 i;
1671
1672         if (!s->get_ctrl && !s->set_ctrl)
1673                 return -EINVAL;
1674
1675         if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
1676                 return -EFAULT;
1677
1678         if (!s->get_ctrl) {
1679                 for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
1680                         if (ctrl.id == s->qctrl[i].id) {
1681                                 ctrl.value = s->_qctrl[i].default_value;
1682                                 goto exit;
1683                         }
1684                 return -EINVAL;
1685         } else
1686                 err = s->get_ctrl(cam, &ctrl);
1687
1688 exit:
1689         if (copy_to_user(arg, &ctrl, sizeof(ctrl)))
1690                 return -EFAULT;
1691
1692         return err;
1693 }
1694
1695
1696 static int
1697 et61x251_vidioc_s_ctrl(struct et61x251_device* cam, void __user * arg)
1698 {
1699         struct et61x251_sensor* s = &cam->sensor;
1700         struct v4l2_control ctrl;
1701         u8 i;
1702         int err = 0;
1703
1704         if (!s->set_ctrl)
1705                 return -EINVAL;
1706
1707         if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
1708                 return -EFAULT;
1709
1710         for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
1711                 if (ctrl.id == s->qctrl[i].id) {
1712                         if (s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)
1713                                 return -EINVAL;
1714                         if (ctrl.value < s->qctrl[i].minimum ||
1715                             ctrl.value > s->qctrl[i].maximum)
1716                                 return -ERANGE;
1717                         ctrl.value -= ctrl.value % s->qctrl[i].step;
1718                         break;
1719                 }
1720
1721         if ((err = s->set_ctrl(cam, &ctrl)))
1722                 return err;
1723
1724         s->_qctrl[i].default_value = ctrl.value;
1725
1726         return 0;
1727 }
1728
1729
1730 static int
1731 et61x251_vidioc_cropcap(struct et61x251_device* cam, void __user * arg)
1732 {
1733         struct v4l2_cropcap* cc = &(cam->sensor.cropcap);
1734
1735         cc->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1736         cc->pixelaspect.numerator = 1;
1737         cc->pixelaspect.denominator = 1;
1738
1739         if (copy_to_user(arg, cc, sizeof(*cc)))
1740                 return -EFAULT;
1741
1742         return 0;
1743 }
1744
1745
1746 static int
1747 et61x251_vidioc_g_crop(struct et61x251_device* cam, void __user * arg)
1748 {
1749         struct et61x251_sensor* s = &cam->sensor;
1750         struct v4l2_crop crop = {
1751                 .type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
1752         };
1753
1754         memcpy(&(crop.c), &(s->_rect), sizeof(struct v4l2_rect));
1755
1756         if (copy_to_user(arg, &crop, sizeof(crop)))
1757                 return -EFAULT;
1758
1759         return 0;
1760 }
1761
1762
1763 static int
1764 et61x251_vidioc_s_crop(struct et61x251_device* cam, void __user * arg)
1765 {
1766         struct et61x251_sensor* s = &cam->sensor;
1767         struct v4l2_crop crop;
1768         struct v4l2_rect* rect;
1769         struct v4l2_rect* bounds = &(s->cropcap.bounds);
1770         struct v4l2_pix_format* pix_format = &(s->pix_format);
1771         u8 scale;
1772         const enum et61x251_stream_state stream = cam->stream;
1773         const u32 nbuffers = cam->nbuffers;
1774         u32 i;
1775         int err = 0;
1776
1777         if (copy_from_user(&crop, arg, sizeof(crop)))
1778                 return -EFAULT;
1779
1780         rect = &(crop.c);
1781
1782         if (crop.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1783                 return -EINVAL;
1784
1785         if (cam->module_param.force_munmap)
1786                 for (i = 0; i < cam->nbuffers; i++)
1787                         if (cam->frame[i].vma_use_count) {
1788                                 DBG(3, "VIDIOC_S_CROP failed. "
1789                                        "Unmap the buffers first.");
1790                                 return -EBUSY;
1791                         }
1792
1793         /* Preserve R,G or B origin */
1794         rect->left = (s->_rect.left & 1L) ? rect->left | 1L : rect->left & ~1L;
1795         rect->top = (s->_rect.top & 1L) ? rect->top | 1L : rect->top & ~1L;
1796
1797         if (rect->width < 16)
1798                 rect->width = 16;
1799         if (rect->height < 16)
1800                 rect->height = 16;
1801         if (rect->width > bounds->width)
1802                 rect->width = bounds->width;
1803         if (rect->height > bounds->height)
1804                 rect->height = bounds->height;
1805         if (rect->left < bounds->left)
1806                 rect->left = bounds->left;
1807         if (rect->top < bounds->top)
1808                 rect->top = bounds->top;
1809         if (rect->left + rect->width > bounds->left + bounds->width)
1810                 rect->left = bounds->left+bounds->width - rect->width;
1811         if (rect->top + rect->height > bounds->top + bounds->height)
1812                 rect->top = bounds->top+bounds->height - rect->height;
1813
1814         rect->width &= ~15L;
1815         rect->height &= ~15L;
1816
1817         if (ET61X251_PRESERVE_IMGSCALE) {
1818                 /* Calculate the actual scaling factor */
1819                 u32 a, b;
1820                 a = rect->width * rect->height;
1821                 b = pix_format->width * pix_format->height;
1822                 scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
1823         } else
1824                 scale = 1;
1825
1826         if (cam->stream == STREAM_ON)
1827                 if ((err = et61x251_stream_interrupt(cam)))
1828                         return err;
1829
1830         if (copy_to_user(arg, &crop, sizeof(crop))) {
1831                 cam->stream = stream;
1832                 return -EFAULT;
1833         }
1834
1835         if (cam->module_param.force_munmap || cam->io == IO_READ)
1836                 et61x251_release_buffers(cam);
1837
1838         err = et61x251_set_crop(cam, rect);
1839         if (s->set_crop)
1840                 err += s->set_crop(cam, rect);
1841         err += et61x251_set_scale(cam, scale);
1842
1843         if (err) { /* atomic, no rollback in ioctl() */
1844                 cam->state |= DEV_MISCONFIGURED;
1845                 DBG(1, "VIDIOC_S_CROP failed because of hardware problems. To "
1846                        "use the camera, close and open /dev/video%d again.",
1847                     cam->v4ldev->minor);
1848                 return -EIO;
1849         }
1850
1851         s->pix_format.width = rect->width/scale;
1852         s->pix_format.height = rect->height/scale;
1853         memcpy(&(s->_rect), rect, sizeof(*rect));
1854
1855         if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
1856             nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
1857                 cam->state |= DEV_MISCONFIGURED;
1858                 DBG(1, "VIDIOC_S_CROP failed because of not enough memory. To "
1859                        "use the camera, close and open /dev/video%d again.",
1860                     cam->v4ldev->minor);
1861                 return -ENOMEM;
1862         }
1863
1864         if (cam->io == IO_READ)
1865                 et61x251_empty_framequeues(cam);
1866         else if (cam->module_param.force_munmap)
1867                 et61x251_requeue_outqueue(cam);
1868
1869         cam->stream = stream;
1870
1871         return 0;
1872 }
1873
1874
1875 static int
1876 et61x251_vidioc_enum_framesizes(struct et61x251_device* cam, void __user * arg)
1877 {
1878         struct v4l2_frmsizeenum frmsize;
1879
1880         if (copy_from_user(&frmsize, arg, sizeof(frmsize)))
1881                 return -EFAULT;
1882
1883         if (frmsize.index != 0)
1884                 return -EINVAL;
1885
1886         if (frmsize.pixel_format != V4L2_PIX_FMT_ET61X251 &&
1887             frmsize.pixel_format != V4L2_PIX_FMT_SBGGR8)
1888                 return -EINVAL;
1889
1890         frmsize.type = V4L2_FRMSIZE_TYPE_STEPWISE;
1891         frmsize.stepwise.min_width = frmsize.stepwise.step_width = 16;
1892         frmsize.stepwise.min_height = frmsize.stepwise.step_height = 16;
1893         frmsize.stepwise.max_width = cam->sensor.cropcap.bounds.width;
1894         frmsize.stepwise.max_height = cam->sensor.cropcap.bounds.height;
1895         memset(&frmsize.reserved, 0, sizeof(frmsize.reserved));
1896
1897         if (copy_to_user(arg, &frmsize, sizeof(frmsize)))
1898                 return -EFAULT;
1899
1900         return 0;
1901 }
1902
1903
1904 static int
1905 et61x251_vidioc_enum_fmt(struct et61x251_device* cam, void __user * arg)
1906 {
1907         struct v4l2_fmtdesc fmtd;
1908
1909         if (copy_from_user(&fmtd, arg, sizeof(fmtd)))
1910                 return -EFAULT;
1911
1912         if (fmtd.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1913                 return -EINVAL;
1914
1915         if (fmtd.index == 0) {
1916                 strcpy(fmtd.description, "bayer rgb");
1917                 fmtd.pixelformat = V4L2_PIX_FMT_SBGGR8;
1918         } else if (fmtd.index == 1) {
1919                 strcpy(fmtd.description, "compressed");
1920                 fmtd.pixelformat = V4L2_PIX_FMT_ET61X251;
1921                 fmtd.flags = V4L2_FMT_FLAG_COMPRESSED;
1922         } else
1923                 return -EINVAL;
1924
1925         fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
1926         memset(&fmtd.reserved, 0, sizeof(fmtd.reserved));
1927
1928         if (copy_to_user(arg, &fmtd, sizeof(fmtd)))
1929                 return -EFAULT;
1930
1931         return 0;
1932 }
1933
1934
1935 static int
1936 et61x251_vidioc_g_fmt(struct et61x251_device* cam, void __user * arg)
1937 {
1938         struct v4l2_format format;
1939         struct v4l2_pix_format* pfmt = &(cam->sensor.pix_format);
1940
1941         if (copy_from_user(&format, arg, sizeof(format)))
1942                 return -EFAULT;
1943
1944         if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1945                 return -EINVAL;
1946
1947         pfmt->colorspace = (pfmt->pixelformat == V4L2_PIX_FMT_ET61X251) ?
1948                            0 : V4L2_COLORSPACE_SRGB;
1949         pfmt->bytesperline = (pfmt->pixelformat==V4L2_PIX_FMT_ET61X251)
1950                              ? 0 : (pfmt->width * pfmt->priv) / 8;
1951         pfmt->sizeimage = pfmt->height * ((pfmt->width*pfmt->priv)/8);
1952         pfmt->field = V4L2_FIELD_NONE;
1953         memcpy(&(format.fmt.pix), pfmt, sizeof(*pfmt));
1954
1955         if (copy_to_user(arg, &format, sizeof(format)))
1956                 return -EFAULT;
1957
1958         return 0;
1959 }
1960
1961
1962 static int
1963 et61x251_vidioc_try_s_fmt(struct et61x251_device* cam, unsigned int cmd,
1964                           void __user * arg)
1965 {
1966         struct et61x251_sensor* s = &cam->sensor;
1967         struct v4l2_format format;
1968         struct v4l2_pix_format* pix;
1969         struct v4l2_pix_format* pfmt = &(s->pix_format);
1970         struct v4l2_rect* bounds = &(s->cropcap.bounds);
1971         struct v4l2_rect rect;
1972         u8 scale;
1973         const enum et61x251_stream_state stream = cam->stream;
1974         const u32 nbuffers = cam->nbuffers;
1975         u32 i;
1976         int err = 0;
1977
1978         if (copy_from_user(&format, arg, sizeof(format)))
1979                 return -EFAULT;
1980
1981         pix = &(format.fmt.pix);
1982
1983         if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
1984                 return -EINVAL;
1985
1986         memcpy(&rect, &(s->_rect), sizeof(rect));
1987
1988         { /* calculate the actual scaling factor */
1989                 u32 a, b;
1990                 a = rect.width * rect.height;
1991                 b = pix->width * pix->height;
1992                 scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
1993         }
1994
1995         rect.width = scale * pix->width;
1996         rect.height = scale * pix->height;
1997
1998         if (rect.width < 16)
1999                 rect.width = 16;
2000         if (rect.height < 16)
2001                 rect.height = 16;
2002         if (rect.width > bounds->left + bounds->width - rect.left)
2003                 rect.width = bounds->left + bounds->width - rect.left;
2004         if (rect.height > bounds->top + bounds->height - rect.top)
2005                 rect.height = bounds->top + bounds->height - rect.top;
2006
2007         rect.width &= ~15L;
2008         rect.height &= ~15L;
2009
2010         { /* adjust the scaling factor */
2011                 u32 a, b;
2012                 a = rect.width * rect.height;
2013                 b = pix->width * pix->height;
2014                 scale = b ? (u8)((a / b) < 4 ? 1 : 2) : 1;
2015         }
2016
2017         pix->width = rect.width / scale;
2018         pix->height = rect.height / scale;
2019
2020         if (pix->pixelformat != V4L2_PIX_FMT_ET61X251 &&
2021             pix->pixelformat != V4L2_PIX_FMT_SBGGR8)
2022                 pix->pixelformat = pfmt->pixelformat;
2023         pix->priv = pfmt->priv; /* bpp */
2024         pix->colorspace = (pix->pixelformat == V4L2_PIX_FMT_ET61X251) ?
2025                           0 : V4L2_COLORSPACE_SRGB;
2026         pix->colorspace = pfmt->colorspace;
2027         pix->bytesperline = (pix->pixelformat == V4L2_PIX_FMT_ET61X251)
2028                             ? 0 : (pix->width * pix->priv) / 8;
2029         pix->sizeimage = pix->height * ((pix->width * pix->priv) / 8);
2030         pix->field = V4L2_FIELD_NONE;
2031
2032         if (cmd == VIDIOC_TRY_FMT) {
2033                 if (copy_to_user(arg, &format, sizeof(format)))
2034                         return -EFAULT;
2035                 return 0;
2036         }
2037
2038         if (cam->module_param.force_munmap)
2039                 for (i = 0; i < cam->nbuffers; i++)
2040                         if (cam->frame[i].vma_use_count) {
2041                                 DBG(3, "VIDIOC_S_FMT failed. "
2042                                        "Unmap the buffers first.");
2043                                 return -EBUSY;
2044                         }
2045
2046         if (cam->stream == STREAM_ON)
2047                 if ((err = et61x251_stream_interrupt(cam)))
2048                         return err;
2049
2050         if (copy_to_user(arg, &format, sizeof(format))) {
2051                 cam->stream = stream;
2052                 return -EFAULT;
2053         }
2054
2055         if (cam->module_param.force_munmap || cam->io == IO_READ)
2056                 et61x251_release_buffers(cam);
2057
2058         err += et61x251_set_pix_format(cam, pix);
2059         err += et61x251_set_crop(cam, &rect);
2060         if (s->set_pix_format)
2061                 err += s->set_pix_format(cam, pix);
2062         if (s->set_crop)
2063                 err += s->set_crop(cam, &rect);
2064         err += et61x251_set_scale(cam, scale);
2065
2066         if (err) { /* atomic, no rollback in ioctl() */
2067                 cam->state |= DEV_MISCONFIGURED;
2068                 DBG(1, "VIDIOC_S_FMT failed because of hardware problems. To "
2069                        "use the camera, close and open /dev/video%d again.",
2070                     cam->v4ldev->minor);
2071                 return -EIO;
2072         }
2073
2074         memcpy(pfmt, pix, sizeof(*pix));
2075         memcpy(&(s->_rect), &rect, sizeof(rect));
2076
2077         if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
2078             nbuffers != et61x251_request_buffers(cam, nbuffers, cam->io)) {
2079                 cam->state |= DEV_MISCONFIGURED;
2080                 DBG(1, "VIDIOC_S_FMT failed because of not enough memory. To "
2081                        "use the camera, close and open /dev/video%d again.",
2082                     cam->v4ldev->minor);
2083                 return -ENOMEM;
2084         }
2085
2086         if (cam->io == IO_READ)
2087                 et61x251_empty_framequeues(cam);
2088         else if (cam->module_param.force_munmap)
2089                 et61x251_requeue_outqueue(cam);
2090
2091         cam->stream = stream;
2092
2093         return 0;
2094 }
2095
2096
2097 static int
2098 et61x251_vidioc_g_jpegcomp(struct et61x251_device* cam, void __user * arg)
2099 {
2100         if (copy_to_user(arg, &cam->compression,
2101                          sizeof(cam->compression)))
2102                 return -EFAULT;
2103
2104         return 0;
2105 }
2106
2107
2108 static int
2109 et61x251_vidioc_s_jpegcomp(struct et61x251_device* cam, void __user * arg)
2110 {
2111         struct v4l2_jpegcompression jc;
2112         const enum et61x251_stream_state stream = cam->stream;
2113         int err = 0;
2114
2115         if (copy_from_user(&jc, arg, sizeof(jc)))
2116                 return -EFAULT;
2117
2118         if (jc.quality != 0 && jc.quality != 1)
2119                 return -EINVAL;
2120
2121         if (cam->stream == STREAM_ON)
2122                 if ((err = et61x251_stream_interrupt(cam)))
2123                         return err;
2124
2125         err += et61x251_set_compression(cam, &jc);
2126         if (err) { /* atomic, no rollback in ioctl() */
2127                 cam->state |= DEV_MISCONFIGURED;
2128                 DBG(1, "VIDIOC_S_JPEGCOMP failed because of hardware "
2129                        "problems. To use the camera, close and open "
2130                        "/dev/video%d again.", cam->v4ldev->minor);
2131                 return -EIO;
2132         }
2133
2134         cam->compression.quality = jc.quality;
2135
2136         cam->stream = stream;
2137
2138         return 0;
2139 }
2140
2141
2142 static int
2143 et61x251_vidioc_reqbufs(struct et61x251_device* cam, void __user * arg)
2144 {
2145         struct v4l2_requestbuffers rb;
2146         u32 i;
2147         int err;
2148
2149         if (copy_from_user(&rb, arg, sizeof(rb)))
2150                 return -EFAULT;
2151
2152         if (rb.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
2153             rb.memory != V4L2_MEMORY_MMAP)
2154                 return -EINVAL;
2155
2156         if (cam->io == IO_READ) {
2157                 DBG(3, "Close and open the device again to choose the mmap "
2158                        "I/O method");
2159                 return -EBUSY;
2160         }
2161
2162         for (i = 0; i < cam->nbuffers; i++)
2163                 if (cam->frame[i].vma_use_count) {
2164                         DBG(3, "VIDIOC_REQBUFS failed. "
2165                                "Previous buffers are still mapped.");
2166                         return -EBUSY;
2167                 }
2168
2169         if (cam->stream == STREAM_ON)
2170                 if ((err = et61x251_stream_interrupt(cam)))
2171                         return err;
2172
2173         et61x251_empty_framequeues(cam);
2174
2175         et61x251_release_buffers(cam);
2176         if (rb.count)
2177                 rb.count = et61x251_request_buffers(cam, rb.count, IO_MMAP);
2178
2179         if (copy_to_user(arg, &rb, sizeof(rb))) {
2180                 et61x251_release_buffers(cam);
2181                 cam->io = IO_NONE;
2182                 return -EFAULT;
2183         }
2184
2185         cam->io = rb.count ? IO_MMAP : IO_NONE;
2186
2187         return 0;
2188 }
2189
2190
2191 static int
2192 et61x251_vidioc_querybuf(struct et61x251_device* cam, void __user * arg)
2193 {
2194         struct v4l2_buffer b;
2195
2196         if (copy_from_user(&b, arg, sizeof(b)))
2197                 return -EFAULT;
2198
2199         if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
2200             b.index >= cam->nbuffers || cam->io != IO_MMAP)
2201                 return -EINVAL;
2202
2203         memcpy(&b, &cam->frame[b.index].buf, sizeof(b));
2204
2205         if (cam->frame[b.index].vma_use_count)
2206                 b.flags |= V4L2_BUF_FLAG_MAPPED;
2207
2208         if (cam->frame[b.index].state == F_DONE)
2209                 b.flags |= V4L2_BUF_FLAG_DONE;
2210         else if (cam->frame[b.index].state != F_UNUSED)
2211                 b.flags |= V4L2_BUF_FLAG_QUEUED;
2212
2213         if (copy_to_user(arg, &b, sizeof(b)))
2214                 return -EFAULT;
2215
2216         return 0;
2217 }
2218
2219
2220 static int
2221 et61x251_vidioc_qbuf(struct et61x251_device* cam, void __user * arg)
2222 {
2223         struct v4l2_buffer b;
2224         unsigned long lock_flags;
2225
2226         if (copy_from_user(&b, arg, sizeof(b)))
2227                 return -EFAULT;
2228
2229         if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
2230             b.index >= cam->nbuffers || cam->io != IO_MMAP)
2231                 return -EINVAL;
2232
2233         if (cam->frame[b.index].state != F_UNUSED)
2234                 return -EINVAL;
2235
2236         cam->frame[b.index].state = F_QUEUED;
2237
2238         spin_lock_irqsave(&cam->queue_lock, lock_flags);
2239         list_add_tail(&cam->frame[b.index].frame, &cam->inqueue);
2240         spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
2241
2242         PDBGG("Frame #%lu queued", (unsigned long)b.index);
2243
2244         return 0;
2245 }
2246
2247
2248 static int
2249 et61x251_vidioc_dqbuf(struct et61x251_device* cam, struct file* filp,
2250                       void __user * arg)
2251 {
2252         struct v4l2_buffer b;
2253         struct et61x251_frame_t *f;
2254         unsigned long lock_flags;
2255         long timeout;
2256
2257         if (copy_from_user(&b, arg, sizeof(b)))
2258                 return -EFAULT;
2259
2260         if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io!= IO_MMAP)
2261                 return -EINVAL;
2262
2263         if (list_empty(&cam->outqueue)) {
2264                 if (cam->stream == STREAM_OFF)
2265                         return -EINVAL;
2266                 if (filp->f_flags & O_NONBLOCK)
2267                         return -EAGAIN;
2268                 timeout = wait_event_interruptible_timeout
2269                           ( cam->wait_frame,
2270                             (!list_empty(&cam->outqueue)) ||
2271                             (cam->state & DEV_DISCONNECTED) ||
2272                             (cam->state & DEV_MISCONFIGURED),
2273                             cam->module_param.frame_timeout *
2274                             1000 * msecs_to_jiffies(1) );
2275                 if (timeout < 0)
2276                         return timeout;
2277                 if (cam->state & DEV_DISCONNECTED)
2278                         return -ENODEV;
2279                 if (!timeout || (cam->state & DEV_MISCONFIGURED))
2280                         return -EIO;
2281         }
2282
2283         spin_lock_irqsave(&cam->queue_lock, lock_flags);
2284         f = list_entry(cam->outqueue.next, struct et61x251_frame_t, frame);
2285         list_del(cam->outqueue.next);
2286         spin_unlock_irqrestore(&cam->queue_lock, lock_flags);
2287
2288         f->state = F_UNUSED;
2289
2290         memcpy(&b, &f->buf, sizeof(b));
2291         if (f->vma_use_count)
2292                 b.flags |= V4L2_BUF_FLAG_MAPPED;
2293
2294         if (copy_to_user(arg, &b, sizeof(b)))
2295                 return -EFAULT;
2296
2297         PDBGG("Frame #%lu dequeued", (unsigned long)f->buf.index);
2298
2299         return 0;
2300 }
2301
2302
2303 static int
2304 et61x251_vidioc_streamon(struct et61x251_device* cam, void __user * arg)
2305 {
2306         int type;
2307
2308         if (copy_from_user(&type, arg, sizeof(type)))
2309                 return -EFAULT;
2310
2311         if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
2312                 return -EINVAL;
2313
2314         cam->stream = STREAM_ON;
2315
2316         DBG(3, "Stream on");
2317
2318         return 0;
2319 }
2320
2321
2322 static int
2323 et61x251_vidioc_streamoff(struct et61x251_device* cam, void __user * arg)
2324 {
2325         int type, err;
2326
2327         if (copy_from_user(&type, arg, sizeof(type)))
2328                 return -EFAULT;
2329
2330         if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
2331                 return -EINVAL;
2332
2333         if (cam->stream == STREAM_ON)
2334                 if ((err = et61x251_stream_interrupt(cam)))
2335                         return err;
2336
2337         et61x251_empty_framequeues(cam);
2338
2339         DBG(3, "Stream off");
2340
2341         return 0;
2342 }
2343
2344
2345 static int
2346 et61x251_vidioc_g_parm(struct et61x251_device* cam, void __user * arg)
2347 {
2348         struct v4l2_streamparm sp;
2349
2350         if (copy_from_user(&sp, arg, sizeof(sp)))
2351                 return -EFAULT;
2352
2353         if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
2354                 return -EINVAL;
2355
2356         sp.parm.capture.extendedmode = 0;
2357         sp.parm.capture.readbuffers = cam->nreadbuffers;
2358
2359         if (copy_to_user(arg, &sp, sizeof(sp)))
2360                 return -EFAULT;
2361
2362         return 0;
2363 }
2364
2365
2366 static int
2367 et61x251_vidioc_s_parm(struct et61x251_device* cam, void __user * arg)
2368 {
2369         struct v4l2_streamparm sp;
2370
2371         if (copy_from_user(&sp, arg, sizeof(sp)))
2372                 return -EFAULT;
2373
2374         if (sp.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
2375                 return -EINVAL;
2376
2377         sp.parm.capture.extendedmode = 0;
2378
2379         if (sp.parm.capture.readbuffers == 0)
2380                 sp.parm.capture.readbuffers = cam->nreadbuffers;
2381
2382         if (sp.parm.capture.readbuffers > ET61X251_MAX_FRAMES)
2383                 sp.parm.capture.readbuffers = ET61X251_MAX_FRAMES;
2384
2385         if (copy_to_user(arg, &sp, sizeof(sp)))
2386                 return -EFAULT;
2387
2388         cam->nreadbuffers = sp.parm.capture.readbuffers;
2389
2390         return 0;
2391 }
2392
2393
2394 static int et61x251_ioctl_v4l2(struct inode* inode, struct file* filp,
2395                                unsigned int cmd, void __user * arg)
2396 {
2397         struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
2398
2399         switch (cmd) {
2400
2401         case VIDIOC_QUERYCAP:
2402                 return et61x251_vidioc_querycap(cam, arg);
2403
2404         case VIDIOC_ENUMINPUT:
2405                 return et61x251_vidioc_enuminput(cam, arg);
2406
2407         case VIDIOC_G_INPUT:
2408                 return et61x251_vidioc_g_input(cam, arg);
2409
2410         case VIDIOC_S_INPUT:
2411                 return et61x251_vidioc_s_input(cam, arg);
2412
2413         case VIDIOC_QUERYCTRL:
2414                 return et61x251_vidioc_query_ctrl(cam, arg);
2415
2416         case VIDIOC_G_CTRL:
2417                 return et61x251_vidioc_g_ctrl(cam, arg);
2418
2419         case VIDIOC_S_CTRL:
2420                 return et61x251_vidioc_s_ctrl(cam, arg);
2421
2422         case VIDIOC_CROPCAP:
2423                 return et61x251_vidioc_cropcap(cam, arg);
2424
2425         case VIDIOC_G_CROP:
2426                 return et61x251_vidioc_g_crop(cam, arg);
2427
2428         case VIDIOC_S_CROP:
2429                 return et61x251_vidioc_s_crop(cam, arg);
2430
2431         case VIDIOC_ENUM_FMT:
2432                 return et61x251_vidioc_enum_fmt(cam, arg);
2433
2434         case VIDIOC_G_FMT:
2435                 return et61x251_vidioc_g_fmt(cam, arg);
2436
2437         case VIDIOC_TRY_FMT:
2438         case VIDIOC_S_FMT:
2439                 return et61x251_vidioc_try_s_fmt(cam, cmd, arg);
2440
2441         case VIDIOC_ENUM_FRAMESIZES:
2442                 return et61x251_vidioc_enum_framesizes(cam, arg);
2443
2444         case VIDIOC_G_JPEGCOMP:
2445                 return et61x251_vidioc_g_jpegcomp(cam, arg);
2446
2447         case VIDIOC_S_JPEGCOMP:
2448                 return et61x251_vidioc_s_jpegcomp(cam, arg);
2449
2450         case VIDIOC_REQBUFS:
2451                 return et61x251_vidioc_reqbufs(cam, arg);
2452
2453         case VIDIOC_QUERYBUF:
2454                 return et61x251_vidioc_querybuf(cam, arg);
2455
2456         case VIDIOC_QBUF:
2457                 return et61x251_vidioc_qbuf(cam, arg);
2458
2459         case VIDIOC_DQBUF:
2460                 return et61x251_vidioc_dqbuf(cam, filp, arg);
2461
2462         case VIDIOC_STREAMON:
2463                 return et61x251_vidioc_streamon(cam, arg);
2464
2465         case VIDIOC_STREAMOFF:
2466                 return et61x251_vidioc_streamoff(cam, arg);
2467
2468         case VIDIOC_G_PARM:
2469                 return et61x251_vidioc_g_parm(cam, arg);
2470
2471         case VIDIOC_S_PARM:
2472                 return et61x251_vidioc_s_parm(cam, arg);
2473
2474         case VIDIOC_G_STD:
2475         case VIDIOC_S_STD:
2476         case VIDIOC_QUERYSTD:
2477         case VIDIOC_ENUMSTD:
2478         case VIDIOC_QUERYMENU:
2479         case VIDIOC_ENUM_FRAMEINTERVALS:
2480                 return -EINVAL;
2481
2482         default:
2483                 return -EINVAL;
2484
2485         }
2486 }
2487
2488
2489 static int et61x251_ioctl(struct inode* inode, struct file* filp,
2490                          unsigned int cmd, unsigned long arg)
2491 {
2492         struct et61x251_device* cam = video_get_drvdata(video_devdata(filp));
2493         int err = 0;
2494
2495         if (mutex_lock_interruptible(&cam->fileop_mutex))
2496                 return -ERESTARTSYS;
2497
2498         if (cam->state & DEV_DISCONNECTED) {
2499                 DBG(1, "Device not present");
2500                 mutex_unlock(&cam->fileop_mutex);
2501                 return -ENODEV;
2502         }
2503
2504         if (cam->state & DEV_MISCONFIGURED) {
2505                 DBG(1, "The camera is misconfigured. Close and open it "
2506                        "again.");
2507                 mutex_unlock(&cam->fileop_mutex);
2508                 return -EIO;
2509         }
2510
2511         V4LDBG(3, "et61x251", cmd);
2512
2513         err = et61x251_ioctl_v4l2(inode, filp, cmd, (void __user *)arg);
2514
2515         mutex_unlock(&cam->fileop_mutex);
2516
2517         return err;
2518 }
2519
2520
2521 static const struct file_operations et61x251_fops = {
2522         .owner = THIS_MODULE,
2523         .open =    et61x251_open,
2524         .release = et61x251_release,
2525         .ioctl =   et61x251_ioctl,
2526 #ifdef CONFIG_COMPAT
2527         .compat_ioctl = v4l_compat_ioctl32,
2528 #endif
2529         .read =    et61x251_read,
2530         .poll =    et61x251_poll,
2531         .mmap =    et61x251_mmap,
2532         .llseek =  no_llseek,
2533 };
2534
2535 /*****************************************************************************/
2536
2537 /* It exists a single interface only. We do not need to validate anything. */
2538 static int
2539 et61x251_usb_probe(struct usb_interface* intf, const struct usb_device_id* id)
2540 {
2541         struct usb_device *udev = interface_to_usbdev(intf);
2542         struct et61x251_device* cam;
2543         static unsigned int dev_nr;
2544         unsigned int i;
2545         int err = 0;
2546
2547         if (!(cam = kzalloc(sizeof(struct et61x251_device), GFP_KERNEL)))
2548                 return -ENOMEM;
2549
2550         cam->usbdev = udev;
2551
2552         if (!(cam->control_buffer = kzalloc(8, GFP_KERNEL))) {
2553                 DBG(1, "kmalloc() failed");
2554                 err = -ENOMEM;
2555                 goto fail;
2556         }
2557
2558         if (!(cam->v4ldev = video_device_alloc())) {
2559                 DBG(1, "video_device_alloc() failed");
2560                 err = -ENOMEM;
2561                 goto fail;
2562         }
2563
2564         DBG(2, "ET61X[12]51 PC Camera Controller detected "
2565                "(vid/pid 0x%04X:0x%04X)",id->idVendor, id->idProduct);
2566
2567         for  (i = 0; et61x251_sensor_table[i]; i++) {
2568                 err = et61x251_sensor_table[i](cam);
2569                 if (!err)
2570                         break;
2571         }
2572
2573         if (!err)
2574                 DBG(2, "%s image sensor detected", cam->sensor.name);
2575         else {
2576                 DBG(1, "No supported image sensor detected");
2577                 err = -ENODEV;
2578                 goto fail;
2579         }
2580
2581         if (et61x251_init(cam)) {
2582                 DBG(1, "Initialization failed. I will retry on open().");
2583                 cam->state |= DEV_MISCONFIGURED;
2584         }
2585
2586         strcpy(cam->v4ldev->name, "ET61X[12]51 PC Camera");
2587         cam->v4ldev->owner = THIS_MODULE;
2588         cam->v4ldev->type = VID_TYPE_CAPTURE | VID_TYPE_SCALES;
2589         cam->v4ldev->fops = &et61x251_fops;
2590         cam->v4ldev->minor = video_nr[dev_nr];
2591         cam->v4ldev->release = video_device_release;
2592         video_set_drvdata(cam->v4ldev, cam);
2593
2594         init_completion(&cam->probe);
2595
2596         err = video_register_device(cam->v4ldev, VFL_TYPE_GRABBER,
2597                                     video_nr[dev_nr]);
2598         if (err) {
2599                 DBG(1, "V4L2 device registration failed");
2600                 if (err == -ENFILE && video_nr[dev_nr] == -1)
2601                         DBG(1, "Free /dev/videoX node not found");
2602                 video_nr[dev_nr] = -1;
2603                 dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
2604                 complete_all(&cam->probe);
2605                 goto fail;
2606         }
2607
2608         DBG(2, "V4L2 device registered as /dev/video%d", cam->v4ldev->minor);
2609
2610         cam->module_param.force_munmap = force_munmap[dev_nr];
2611         cam->module_param.frame_timeout = frame_timeout[dev_nr];
2612
2613         dev_nr = (dev_nr < ET61X251_MAX_DEVICES-1) ? dev_nr+1 : 0;
2614
2615 #ifdef CONFIG_VIDEO_ADV_DEBUG
2616         err = et61x251_create_sysfs(cam);
2617         if (!err)
2618                 DBG(2, "Optional device control through 'sysfs' "
2619                        "interface ready");
2620         else
2621                 DBG(2, "Failed to create 'sysfs' interface for optional "
2622                        "device controlling. Error #%d", err);
2623 #else
2624         DBG(2, "Optional device control through 'sysfs' interface disabled");
2625         DBG(3, "Compile the kernel with the 'CONFIG_VIDEO_ADV_DEBUG' "
2626                "configuration option to enable it.");
2627 #endif
2628
2629         usb_set_intfdata(intf, cam);
2630         kref_init(&cam->kref);
2631         usb_get_dev(cam->usbdev);
2632
2633         complete_all(&cam->probe);
2634
2635         return 0;
2636
2637 fail:
2638         if (cam) {
2639                 kfree(cam->control_buffer);
2640                 if (cam->v4ldev)
2641                         video_device_release(cam->v4ldev);
2642                 kfree(cam);
2643         }
2644         return err;
2645 }
2646
2647
2648 static void et61x251_usb_disconnect(struct usb_interface* intf)
2649 {
2650         struct et61x251_device* cam;
2651
2652         down_write(&et61x251_dev_lock);
2653
2654         cam = usb_get_intfdata(intf);
2655
2656         DBG(2, "Disconnecting %s...", cam->v4ldev->name);
2657
2658         if (cam->users) {
2659                 DBG(2, "Device /dev/video%d is open! Deregistration and "
2660                        "memory deallocation are deferred.",
2661                     cam->v4ldev->minor);
2662                 cam->state |= DEV_MISCONFIGURED;
2663                 et61x251_stop_transfer(cam);
2664                 cam->state |= DEV_DISCONNECTED;
2665                 wake_up_interruptible(&cam->wait_frame);
2666                 wake_up(&cam->wait_stream);
2667         } else
2668                 cam->state |= DEV_DISCONNECTED;
2669
2670         wake_up_interruptible_all(&cam->wait_open);
2671
2672         kref_put(&cam->kref, et61x251_release_resources);
2673
2674         up_write(&et61x251_dev_lock);
2675 }
2676
2677
2678 static struct usb_driver et61x251_usb_driver = {
2679         .name =       "et61x251",
2680         .id_table =   et61x251_id_table,
2681         .probe =      et61x251_usb_probe,
2682         .disconnect = et61x251_usb_disconnect,
2683 };
2684
2685 /*****************************************************************************/
2686
2687 static int __init et61x251_module_init(void)
2688 {
2689         int err = 0;
2690
2691         KDBG(2, ET61X251_MODULE_NAME " v" ET61X251_MODULE_VERSION);
2692         KDBG(3, ET61X251_MODULE_AUTHOR);
2693
2694         if ((err = usb_register(&et61x251_usb_driver)))
2695                 KDBG(1, "usb_register() failed");
2696
2697         return err;
2698 }
2699
2700
2701 static void __exit et61x251_module_exit(void)
2702 {
2703         usb_deregister(&et61x251_usb_driver);
2704 }
2705
2706
2707 module_init(et61x251_module_init);
2708 module_exit(et61x251_module_exit);