drivers/usb/media/et61x251_core.c

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