drivers/media/usb/cpia2/cpia2_usb.c

   1 /****************************************************************************
   2  *
   3  *  Filename: cpia2_usb.c
   4  *
   5  *  Copyright 2001, STMicrolectronics, Inc.
   6  *      Contact:  steve.miller@st.com
   7  *
   8  *  Description:
   9  *     This is a USB driver for CPia2 based video cameras.
  10  *     The infrastructure of this driver is based on the cpia usb driver by
  11  *     Jochen Scharrlach and Johannes Erdfeldt.
  12  *
  13  *  This program is free software; you can redistribute it and/or modify
  14  *  it under the terms of the GNU General Public License as published by
  15  *  the Free Software Foundation; either version 2 of the License, or
  16  *  (at your option) any later version.
  17  *
  18  *  This program is distributed in the hope that it will be useful,
  19  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  20  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  21  *  GNU General Public License for more details.
  22  *
  23  *  You should have received a copy of the GNU General Public License
  24  *  along with this program; if not, write to the Free Software
  25  *  Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  26  *
  27  *  Stripped of 2.4 stuff ready for main kernel submit by
  28  *              Alan Cox <alan@lxorguk.ukuu.org.uk>
  29  ****************************************************************************/
  30
  31 #include <linux/kernel.h>
  32 #include <linux/slab.h>
  33 #include <linux/usb.h>
  34 #include <linux/module.h>
  35
  36 #include "cpia2.h"
  37
  38 static int frame_sizes[] = {
  39         0,      // USBIF_CMDONLY
  40         0,      // USBIF_BULK
  41         128,    // USBIF_ISO_1
  42         384,    // USBIF_ISO_2
  43         640,    // USBIF_ISO_3
  44         768,    // USBIF_ISO_4
  45         896,    // USBIF_ISO_5
  46         1023,   // USBIF_ISO_6
  47 };
  48
  49 #define FRAMES_PER_DESC    10
  50 #define FRAME_SIZE_PER_DESC   frame_sizes[cam->cur_alt]
  51
  52 static void process_frame(struct camera_data *cam);
  53 static void cpia2_usb_complete(struct urb *urb);
  54 static int cpia2_usb_probe(struct usb_interface *intf,
  55                            const struct usb_device_id *id);
  56 static void cpia2_usb_disconnect(struct usb_interface *intf);
  57 static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message);
  58 static int cpia2_usb_resume(struct usb_interface *intf);
  59
  60 static void free_sbufs(struct camera_data *cam);
  61 static void add_APPn(struct camera_data *cam);
  62 static void add_COM(struct camera_data *cam);
  63 static int submit_urbs(struct camera_data *cam);
  64 static int set_alternate(struct camera_data *cam, unsigned int alt);
  65 static int configure_transfer_mode(struct camera_data *cam, unsigned int alt);
  66
  67 static struct usb_device_id cpia2_id_table[] = {
  68         {USB_DEVICE(0x0553, 0x0100)},
  69         {USB_DEVICE(0x0553, 0x0140)},
  70         {USB_DEVICE(0x0553, 0x0151)},  /* STV0676 */
  71         {}                      /* Terminating entry */
  72 };
  73 MODULE_DEVICE_TABLE(usb, cpia2_id_table);
  74
  75 static struct usb_driver cpia2_driver = {
  76         .name           = "cpia2",
  77         .probe          = cpia2_usb_probe,
  78         .disconnect     = cpia2_usb_disconnect,
  79         .suspend        = cpia2_usb_suspend,
  80         .resume         = cpia2_usb_resume,
  81         .reset_resume   = cpia2_usb_resume,
  82         .id_table       = cpia2_id_table
  83 };
  84
  85
  86 /******************************************************************************
  87  *
  88  *  process_frame
  89  *
  90  *****************************************************************************/
  91 static void process_frame(struct camera_data *cam)
  92 {
  93         static int frame_count;
  94
  95         unsigned char *inbuff = cam->workbuff->data;
  96
  97         DBG("Processing frame #%d, current:%d\n",
  98             cam->workbuff->num, cam->curbuff->num);
  99
 100         if(cam->workbuff->length > cam->workbuff->max_length)
 101                 cam->workbuff->max_length = cam->workbuff->length;
 102
 103         if ((inbuff[0] == 0xFF) && (inbuff[1] == 0xD8)) {
 104                 frame_count++;
 105         } else {
 106                 cam->workbuff->status = FRAME_ERROR;
 107                 DBG("Start of frame not found\n");
 108                 return;
 109         }
 110
 111         /***
 112          * Now the output buffer should have a JPEG image in it.
 113          ***/
 114         if(!cam->first_image_seen) {
 115                 /* Always skip the first image after streaming
 116                  * starts. It is almost certainly corrupt. */
 117                 cam->first_image_seen = 1;
 118                 cam->workbuff->status = FRAME_EMPTY;
 119                 return;
 120         }
 121         if (cam->workbuff->length > 3) {
 122                 if(cam->mmapped &&
 123                    cam->workbuff->length < cam->workbuff->max_length) {
 124                         /* No junk in the buffers */
 125                         memset(cam->workbuff->data+cam->workbuff->length,
 126                                0, cam->workbuff->max_length-
 127                                   cam->workbuff->length);
 128                 }
 129                 cam->workbuff->max_length = cam->workbuff->length;
 130                 cam->workbuff->status = FRAME_READY;
 131
 132                 if(!cam->mmapped && cam->num_frames > 2) {
 133                         /* During normal reading, the most recent
 134                          * frame will be read.  If the current frame
 135                          * hasn't started reading yet, it will never
 136                          * be read, so mark it empty.  If the buffer is
 137                          * mmapped, or we have few buffers, we need to
 138                          * wait for the user to free the buffer.
 139                          *
 140                          * NOTE: This is not entirely foolproof with 3
 141                          * buffers, but it would take an EXTREMELY
 142                          * overloaded system to cause problems (possible
 143                          * image data corruption).  Basically, it would
 144                          * need to take more time to execute cpia2_read
 145                          * than it would for the camera to send
 146                          * cam->num_frames-2 frames before problems
 147                          * could occur.
 148                          */
 149                         cam->curbuff->status = FRAME_EMPTY;
 150                 }
 151                 cam->curbuff = cam->workbuff;
 152                 cam->workbuff = cam->workbuff->next;
 153                 DBG("Changed buffers, work:%d, current:%d\n",
 154                     cam->workbuff->num, cam->curbuff->num);
 155                 return;
 156         } else {
 157                 DBG("Not enough data for an image.\n");
 158         }
 159
 160         cam->workbuff->status = FRAME_ERROR;
 161         return;
 162 }
 163
 164 /******************************************************************************
 165  *
 166  *  add_APPn
 167  *
 168  *  Adds a user specified APPn record
 169  *****************************************************************************/
 170 static void add_APPn(struct camera_data *cam)
 171 {
 172         if(cam->APP_len > 0) {
 173                 cam->workbuff->data[cam->workbuff->length++] = 0xFF;
 174                 cam->workbuff->data[cam->workbuff->length++] = 0xE0+cam->APPn;
 175                 cam->workbuff->data[cam->workbuff->length++] = 0;
 176                 cam->workbuff->data[cam->workbuff->length++] = cam->APP_len+2;
 177                 memcpy(cam->workbuff->data+cam->workbuff->length,
 178                        cam->APP_data, cam->APP_len);
 179                 cam->workbuff->length += cam->APP_len;
 180         }
 181 }
 182
 183 /******************************************************************************
 184  *
 185  *  add_COM
 186  *
 187  *  Adds a user specified COM record
 188  *****************************************************************************/
 189 static void add_COM(struct camera_data *cam)
 190 {
 191         if(cam->COM_len > 0) {
 192                 cam->workbuff->data[cam->workbuff->length++] = 0xFF;
 193                 cam->workbuff->data[cam->workbuff->length++] = 0xFE;
 194                 cam->workbuff->data[cam->workbuff->length++] = 0;
 195                 cam->workbuff->data[cam->workbuff->length++] = cam->COM_len+2;
 196                 memcpy(cam->workbuff->data+cam->workbuff->length,
 197                        cam->COM_data, cam->COM_len);
 198                 cam->workbuff->length += cam->COM_len;
 199         }
 200 }
 201
 202 /******************************************************************************
 203  *
 204  *  cpia2_usb_complete
 205  *
 206  *  callback when incoming packet is received
 207  *****************************************************************************/
 208 static void cpia2_usb_complete(struct urb *urb)
 209 {
 210         int i;
 211         unsigned char *cdata;
 212         static bool frame_ready = false;
 213         struct camera_data *cam = (struct camera_data *) urb->context;
 214
 215         if (urb->status!=0) {
 216                 if (!(urb->status == -ENOENT ||
 217                       urb->status == -ECONNRESET ||
 218                       urb->status == -ESHUTDOWN))
 219                 {
 220                         DBG("urb->status = %d!\n", urb->status);
 221                 }
 222                 DBG("Stopping streaming\n");
 223                 return;
 224         }
 225
 226         if (!cam->streaming || !video_is_registered(&cam->vdev)) {
 227                 LOG("Will now stop the streaming: streaming = %d, present=%d\n",
 228                     cam->streaming, video_is_registered(&cam->vdev));
 229                 return;
 230         }
 231
 232         /***
 233          * Packet collater
 234          ***/
 235         //DBG("Collating %d packets\n", urb->number_of_packets);
 236         for (i = 0; i < urb->number_of_packets; i++) {
 237                 u16 checksum, iso_checksum;
 238                 int j;
 239                 int n = urb->iso_frame_desc[i].actual_length;
 240                 int st = urb->iso_frame_desc[i].status;
 241
 242                 if(cam->workbuff->status == FRAME_READY) {
 243                         struct framebuf *ptr;
 244                         /* Try to find an available buffer */
 245                         DBG("workbuff full, searching\n");
 246                         for (ptr = cam->workbuff->next;
 247                              ptr != cam->workbuff;
 248                              ptr = ptr->next)
 249                         {
 250                                 if (ptr->status == FRAME_EMPTY) {
 251                                         ptr->status = FRAME_READING;
 252                                         ptr->length = 0;
 253                                         break;
 254                                 }
 255                         }
 256                         if (ptr == cam->workbuff)
 257                                 break; /* No READING or EMPTY buffers left */
 258
 259                         cam->workbuff = ptr;
 260                 }
 261
 262                 if (cam->workbuff->status == FRAME_EMPTY ||
 263                     cam->workbuff->status == FRAME_ERROR) {
 264                         cam->workbuff->status = FRAME_READING;
 265                         cam->workbuff->length = 0;
 266                 }
 267
 268                 //DBG("   Packet %d length = %d, status = %d\n", i, n, st);
 269                 cdata = urb->transfer_buffer + urb->iso_frame_desc[i].offset;
 270
 271                 if (st) {
 272                         LOG("cpia2 data error: [%d] len=%d, status = %d\n",
 273                             i, n, st);
 274                         if(!ALLOW_CORRUPT)
 275                                 cam->workbuff->status = FRAME_ERROR;
 276                         continue;
 277                 }
 278
 279                 if(n<=2)
 280                         continue;
 281
 282                 checksum = 0;
 283                 for(j=0; j<n-2; ++j)
 284                         checksum += cdata[j];
 285                 iso_checksum = cdata[j] + cdata[j+1]*256;
 286                 if(checksum != iso_checksum) {
 287                         LOG("checksum mismatch: [%d] len=%d, calculated = %x, checksum = %x\n",
 288                             i, n, (int)checksum, (int)iso_checksum);
 289                         if(!ALLOW_CORRUPT) {
 290                                 cam->workbuff->status = FRAME_ERROR;
 291                                 continue;
 292                         }
 293                 }
 294                 n -= 2;
 295
 296                 if(cam->workbuff->status != FRAME_READING) {
 297                         if((0xFF == cdata[0] && 0xD8 == cdata[1]) ||
 298                            (0xD8 == cdata[0] && 0xFF == cdata[1] &&
 299                             0 != cdata[2])) {
 300                                 /* frame is skipped, but increment total
 301                                  * frame count anyway */
 302                                 cam->frame_count++;
 303                         }
 304                         DBG("workbuff not reading, status=%d\n",
 305                             cam->workbuff->status);
 306                         continue;
 307                 }
 308
 309                 if (cam->frame_size < cam->workbuff->length + n) {
 310                         ERR("buffer overflow! length: %d, n: %d\n",
 311                             cam->workbuff->length, n);
 312                         cam->workbuff->status = FRAME_ERROR;
 313                         if(cam->workbuff->length > cam->workbuff->max_length)
 314                                 cam->workbuff->max_length =
 315                                         cam->workbuff->length;
 316                         continue;
 317                 }
 318
 319                 if (cam->workbuff->length == 0) {
 320                         int data_offset;
 321                         if ((0xD8 == cdata[0]) && (0xFF == cdata[1])) {
 322                                 data_offset = 1;
 323                         } else if((0xFF == cdata[0]) && (0xD8 == cdata[1])
 324                                   && (0xFF == cdata[2])) {
 325                                 data_offset = 2;
 326                         } else {
 327                                 DBG("Ignoring packet, not beginning!\n");
 328                                 continue;
 329                         }
 330                         DBG("Start of frame pattern found\n");
 331                         v4l2_get_timestamp(&cam->workbuff->timestamp);
 332                         cam->workbuff->seq = cam->frame_count++;
 333                         cam->workbuff->data[0] = 0xFF;
 334                         cam->workbuff->data[1] = 0xD8;
 335                         cam->workbuff->length = 2;
 336                         add_APPn(cam);
 337                         add_COM(cam);
 338                         memcpy(cam->workbuff->data+cam->workbuff->length,
 339                                cdata+data_offset, n-data_offset);
 340                         cam->workbuff->length += n-data_offset;
 341                 } else if (cam->workbuff->length > 0) {
 342                         memcpy(cam->workbuff->data + cam->workbuff->length,
 343                                cdata, n);
 344                         cam->workbuff->length += n;
 345                 }
 346
 347                 if ((cam->workbuff->length >= 3) &&
 348                     (cam->workbuff->data[cam->workbuff->length - 3] == 0xFF) &&
 349                     (cam->workbuff->data[cam->workbuff->length - 2] == 0xD9) &&
 350                     (cam->workbuff->data[cam->workbuff->length - 1] == 0xFF)) {
 351                         frame_ready = true;
 352                         cam->workbuff->data[cam->workbuff->length - 1] = 0;
 353                         cam->workbuff->length -= 1;
 354                 } else if ((cam->workbuff->length >= 2) &&
 355                    (cam->workbuff->data[cam->workbuff->length - 2] == 0xFF) &&
 356                    (cam->workbuff->data[cam->workbuff->length - 1] == 0xD9)) {
 357                         frame_ready = true;
 358                 }
 359
 360                 if (frame_ready) {
 361                         DBG("Workbuff image size = %d\n",cam->workbuff->length);
 362                         process_frame(cam);
 363
 364                         frame_ready = false;
 365
 366                         if (waitqueue_active(&cam->wq_stream))
 367                                 wake_up_interruptible(&cam->wq_stream);
 368                 }
 369         }
 370
 371         if(cam->streaming) {
 372                 /* resubmit */
 373                 urb->dev = cam->dev;
 374                 if ((i = usb_submit_urb(urb, GFP_ATOMIC)) != 0)
 375                         ERR("%s: usb_submit_urb ret %d!\n", __func__, i);
 376         }
 377 }
 378
 379 /******************************************************************************
 380  *
 381  * configure_transfer_mode
 382  *
 383  *****************************************************************************/
 384 static int configure_transfer_mode(struct camera_data *cam, unsigned int alt)
 385 {
 386         static unsigned char iso_regs[8][4] = {
 387                 {0x00, 0x00, 0x00, 0x00},
 388                 {0x00, 0x00, 0x00, 0x00},
 389                 {0xB9, 0x00, 0x00, 0x7E},
 390                 {0xB9, 0x00, 0x01, 0x7E},
 391                 {0xB9, 0x00, 0x02, 0x7E},
 392                 {0xB9, 0x00, 0x02, 0xFE},
 393                 {0xB9, 0x00, 0x03, 0x7E},
 394                 {0xB9, 0x00, 0x03, 0xFD}
 395         };
 396         struct cpia2_command cmd;
 397         unsigned char reg;
 398
 399         if (!video_is_registered(&cam->vdev))
 400                 return -ENODEV;
 401
 402         /***
 403          * Write the isoc registers according to the alternate selected
 404          ***/
 405         cmd.direction = TRANSFER_WRITE;
 406         cmd.buffer.block_data[0] = iso_regs[alt][0];
 407         cmd.buffer.block_data[1] = iso_regs[alt][1];
 408         cmd.buffer.block_data[2] = iso_regs[alt][2];
 409         cmd.buffer.block_data[3] = iso_regs[alt][3];
 410         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 411         cmd.start = CPIA2_VC_USB_ISOLIM;
 412         cmd.reg_count = 4;
 413         cpia2_send_command(cam, &cmd);
 414
 415         /***
 416          * Enable relevant streams before starting polling.
 417          * First read USB Stream Config Register.
 418          ***/
 419         cmd.direction = TRANSFER_READ;
 420         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 421         cmd.start = CPIA2_VC_USB_STRM;
 422         cmd.reg_count = 1;
 423         cpia2_send_command(cam, &cmd);
 424         reg = cmd.buffer.block_data[0];
 425
 426         /* Clear iso, bulk, and int */
 427         reg &= ~(CPIA2_VC_USB_STRM_BLK_ENABLE |
 428                  CPIA2_VC_USB_STRM_ISO_ENABLE |
 429                  CPIA2_VC_USB_STRM_INT_ENABLE);
 430
 431         if (alt == USBIF_BULK) {
 432                 DBG("Enabling bulk xfer\n");
 433                 reg |= CPIA2_VC_USB_STRM_BLK_ENABLE;    /* Enable Bulk */
 434                 cam->xfer_mode = XFER_BULK;
 435         } else if (alt >= USBIF_ISO_1) {
 436                 DBG("Enabling ISOC xfer\n");
 437                 reg |= CPIA2_VC_USB_STRM_ISO_ENABLE;
 438                 cam->xfer_mode = XFER_ISOC;
 439         }
 440
 441         cmd.buffer.block_data[0] = reg;
 442         cmd.direction = TRANSFER_WRITE;
 443         cmd.start = CPIA2_VC_USB_STRM;
 444         cmd.reg_count = 1;
 445         cmd.req_mode = CAMERAACCESS_TYPE_BLOCK | CAMERAACCESS_VC;
 446         cpia2_send_command(cam, &cmd);
 447
 448         return 0;
 449 }
 450
 451 /******************************************************************************
 452  *
 453  * cpia2_usb_change_streaming_alternate
 454  *
 455  *****************************************************************************/
 456 int cpia2_usb_change_streaming_alternate(struct camera_data *cam,
 457                                          unsigned int alt)
 458 {
 459         int ret = 0;
 460
 461         if(alt < USBIF_ISO_1 || alt > USBIF_ISO_6)
 462                 return -EINVAL;
 463
 464         if(alt == cam->params.camera_state.stream_mode)
 465                 return 0;
 466
 467         cpia2_usb_stream_pause(cam);
 468
 469         configure_transfer_mode(cam, alt);
 470
 471         cam->params.camera_state.stream_mode = alt;
 472
 473         /* Reset the camera to prevent image quality degradation */
 474         cpia2_reset_camera(cam);
 475
 476         cpia2_usb_stream_resume(cam);
 477
 478         return ret;
 479 }
 480
 481 /******************************************************************************
 482  *
 483  * set_alternate
 484  *
 485  *****************************************************************************/
 486 static int set_alternate(struct camera_data *cam, unsigned int alt)
 487 {
 488         int ret = 0;
 489
 490         if(alt == cam->cur_alt)
 491                 return 0;
 492
 493         if (cam->cur_alt != USBIF_CMDONLY) {
 494                 DBG("Changing from alt %d to %d\n", cam->cur_alt, USBIF_CMDONLY);
 495                 ret = usb_set_interface(cam->dev, cam->iface, USBIF_CMDONLY);
 496                 if (ret != 0)
 497                         return ret;
 498         }
 499         if (alt != USBIF_CMDONLY) {
 500                 DBG("Changing from alt %d to %d\n", USBIF_CMDONLY, alt);
 501                 ret = usb_set_interface(cam->dev, cam->iface, alt);
 502                 if (ret != 0)
 503                         return ret;
 504         }
 505
 506         cam->old_alt = cam->cur_alt;
 507         cam->cur_alt = alt;
 508
 509         return ret;
 510 }
 511
 512 /******************************************************************************
 513  *
 514  * free_sbufs
 515  *
 516  * Free all cam->sbuf[]. All non-NULL .data and .urb members that are non-NULL
 517  * are assumed to be allocated. Non-NULL .urb members are also assumed to be
 518  * submitted (and must therefore be killed before they are freed).
 519  *****************************************************************************/
 520 static void free_sbufs(struct camera_data *cam)
 521 {
 522         int i;
 523
 524         for (i = 0; i < NUM_SBUF; i++) {
 525                 if(cam->sbuf[i].urb) {
 526                         usb_kill_urb(cam->sbuf[i].urb);
 527                         usb_free_urb(cam->sbuf[i].urb);
 528                         cam->sbuf[i].urb = NULL;
 529                 }
 530                 if(cam->sbuf[i].data) {
 531                         kfree(cam->sbuf[i].data);
 532                         cam->sbuf[i].data = NULL;
 533                 }
 534         }
 535 }
 536
 537 /*******
 538 * Convenience functions
 539 *******/
 540 /****************************************************************************
 541  *
 542  *  write_packet
 543  *
 544  ***************************************************************************/
 545 static int write_packet(struct usb_device *udev,
 546                         u8 request, u8 * registers, u16 start, size_t size)
 547 {
 548         if (!registers || size <= 0)
 549                 return -EINVAL;
 550
 551         return usb_control_msg(udev,
 552                                usb_sndctrlpipe(udev, 0),
 553                                request,
 554                                USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 555                                start,   /* value */
 556                                0,       /* index */
 557                                registers,       /* buffer */
 558                                size,
 559                                HZ);
 560 }
 561
 562 /****************************************************************************
 563  *
 564  *  read_packet
 565  *
 566  ***************************************************************************/
 567 static int read_packet(struct usb_device *udev,
 568                        u8 request, u8 * registers, u16 start, size_t size)
 569 {
 570         if (!registers || size <= 0)
 571                 return -EINVAL;
 572
 573         return usb_control_msg(udev,
 574                                usb_rcvctrlpipe(udev, 0),
 575                                request,
 576                                USB_DIR_IN|USB_TYPE_VENDOR|USB_RECIP_DEVICE,
 577                                start,   /* value */
 578                                0,       /* index */
 579                                registers,       /* buffer */
 580                                size,
 581                                HZ);
 582 }
 583
 584 /******************************************************************************
 585  *
 586  *  cpia2_usb_transfer_cmd
 587  *
 588  *****************************************************************************/
 589 int cpia2_usb_transfer_cmd(struct camera_data *cam,
 590                            void *registers,
 591                            u8 request, u8 start, u8 count, u8 direction)
 592 {
 593         int err = 0;
 594         struct usb_device *udev = cam->dev;
 595
 596         if (!udev) {
 597                 ERR("%s: Internal driver error: udev is NULL\n", __func__);
 598                 return -EINVAL;
 599         }
 600
 601         if (!registers) {
 602                 ERR("%s: Internal driver error: register array is NULL\n", __func__);
 603                 return -EINVAL;
 604         }
 605
 606         if (direction == TRANSFER_READ) {
 607                 err = read_packet(udev, request, (u8 *)registers, start, count);
 608                 if (err > 0)
 609                         err = 0;
 610         } else if (direction == TRANSFER_WRITE) {
 611                 err =write_packet(udev, request, (u8 *)registers, start, count);
 612                 if (err < 0) {
 613                         LOG("Control message failed, err val = %d\n", err);
 614                         LOG("Message: request = 0x%0X, start = 0x%0X\n",
 615                             request, start);
 616                         LOG("Message: count = %d, register[0] = 0x%0X\n",
 617                             count, ((unsigned char *) registers)[0]);
 618                 } else
 619                         err=0;
 620         } else {
 621                 LOG("Unexpected first byte of direction: %d\n",
 622                        direction);
 623                 return -EINVAL;
 624         }
 625
 626         if(err != 0)
 627                 LOG("Unexpected error: %d\n", err);
 628         return err;
 629 }
 630
 631
 632 /******************************************************************************
 633  *
 634  *  submit_urbs
 635  *
 636  *****************************************************************************/
 637 static int submit_urbs(struct camera_data *cam)
 638 {
 639         struct urb *urb;
 640         int fx, err, i, j;
 641
 642         for(i=0; i<NUM_SBUF; ++i) {
 643                 if (cam->sbuf[i].data)
 644                         continue;
 645                 cam->sbuf[i].data =
 646                     kmalloc(FRAMES_PER_DESC * FRAME_SIZE_PER_DESC, GFP_KERNEL);
 647                 if (!cam->sbuf[i].data) {
 648                         while (--i >= 0) {
 649                                 kfree(cam->sbuf[i].data);
 650                                 cam->sbuf[i].data = NULL;
 651                         }
 652                         return -ENOMEM;
 653                 }
 654         }
 655
 656         /* We double buffer the Isoc lists, and also know the polling
 657          * interval is every frame (1 == (1 << (bInterval -1))).
 658          */
 659         for(i=0; i<NUM_SBUF; ++i) {
 660                 if(cam->sbuf[i].urb) {
 661                         continue;
 662                 }
 663                 urb = usb_alloc_urb(FRAMES_PER_DESC, GFP_KERNEL);
 664                 if (!urb) {
 665                         for (j = 0; j < i; j++)
 666                                 usb_free_urb(cam->sbuf[j].urb);
 667                         return -ENOMEM;
 668                 }
 669
 670                 cam->sbuf[i].urb = urb;
 671                 urb->dev = cam->dev;
 672                 urb->context = cam;
 673                 urb->pipe = usb_rcvisocpipe(cam->dev, 1 /*ISOC endpoint*/);
 674                 urb->transfer_flags = URB_ISO_ASAP;
 675                 urb->transfer_buffer = cam->sbuf[i].data;
 676                 urb->complete = cpia2_usb_complete;
 677                 urb->number_of_packets = FRAMES_PER_DESC;
 678                 urb->interval = 1;
 679                 urb->transfer_buffer_length =
 680                         FRAME_SIZE_PER_DESC * FRAMES_PER_DESC;
 681
 682                 for (fx = 0; fx < FRAMES_PER_DESC; fx++) {
 683                         urb->iso_frame_desc[fx].offset =
 684                                 FRAME_SIZE_PER_DESC * fx;
 685                         urb->iso_frame_desc[fx].length = FRAME_SIZE_PER_DESC;
 686                 }
 687         }
 688
 689
 690         /* Queue the ISO urbs, and resubmit in the completion handler */
 691         for(i=0; i<NUM_SBUF; ++i) {
 692                 err = usb_submit_urb(cam->sbuf[i].urb, GFP_KERNEL);
 693                 if (err) {
 694                         ERR("usb_submit_urb[%d]() = %d\n", i, err);
 695                         return err;
 696                 }
 697         }
 698
 699         return 0;
 700 }
 701
 702 /******************************************************************************
 703  *
 704  *  cpia2_usb_stream_start
 705  *
 706  *****************************************************************************/
 707 int cpia2_usb_stream_start(struct camera_data *cam, unsigned int alternate)
 708 {
 709         int ret;
 710         int old_alt;
 711
 712         if(cam->streaming)
 713                 return 0;
 714
 715         if (cam->flush) {
 716                 int i;
 717                 DBG("Flushing buffers\n");
 718                 for(i=0; i<cam->num_frames; ++i) {
 719                         cam->buffers[i].status = FRAME_EMPTY;
 720                         cam->buffers[i].length = 0;
 721                 }
 722                 cam->curbuff = &cam->buffers[0];
 723                 cam->workbuff = cam->curbuff->next;
 724                 cam->flush = false;
 725         }
 726
 727         old_alt = cam->params.camera_state.stream_mode;
 728         cam->params.camera_state.stream_mode = 0;
 729         ret = cpia2_usb_change_streaming_alternate(cam, alternate);
 730         if (ret < 0) {
 731                 int ret2;
 732                 ERR("cpia2_usb_change_streaming_alternate() = %d!\n", ret);
 733                 cam->params.camera_state.stream_mode = old_alt;
 734                 ret2 = set_alternate(cam, USBIF_CMDONLY);
 735                 if (ret2 < 0) {
 736                         ERR("cpia2_usb_change_streaming_alternate(%d) =%d has already "
 737                             "failed. Then tried to call "
 738                             "set_alternate(USBIF_CMDONLY) = %d.\n",
 739                             alternate, ret, ret2);
 740                 }
 741         } else {
 742                 cam->frame_count = 0;
 743                 cam->streaming = 1;
 744                 ret = cpia2_usb_stream_resume(cam);
 745         }
 746         return ret;
 747 }
 748
 749 /******************************************************************************
 750  *
 751  *  cpia2_usb_stream_pause
 752  *
 753  *****************************************************************************/
 754 int cpia2_usb_stream_pause(struct camera_data *cam)
 755 {
 756         int ret = 0;
 757         if(cam->streaming) {
 758                 free_sbufs(cam);
 759                 ret = set_alternate(cam, USBIF_CMDONLY);
 760         }
 761         return ret;
 762 }
 763
 764 /******************************************************************************
 765  *
 766  *  cpia2_usb_stream_resume
 767  *
 768  *****************************************************************************/
 769 int cpia2_usb_stream_resume(struct camera_data *cam)
 770 {
 771         int ret = 0;
 772         if(cam->streaming) {
 773                 cam->first_image_seen = 0;
 774                 ret = set_alternate(cam, cam->params.camera_state.stream_mode);
 775                 if(ret == 0) {
 776                         /* for some reason the user effects need to be set
 777                            again when starting streaming. */
 778                         cpia2_do_command(cam, CPIA2_CMD_SET_USER_EFFECTS, TRANSFER_WRITE,
 779                                         cam->params.vp_params.user_effects);
 780                         ret = submit_urbs(cam);
 781                 }
 782         }
 783         return ret;
 784 }
 785
 786 /******************************************************************************
 787  *
 788  *  cpia2_usb_stream_stop
 789  *
 790  *****************************************************************************/
 791 int cpia2_usb_stream_stop(struct camera_data *cam)
 792 {
 793         int ret;
 794
 795         ret = cpia2_usb_stream_pause(cam);
 796         cam->streaming = 0;
 797         configure_transfer_mode(cam, 0);
 798         return ret;
 799 }
 800
 801 /******************************************************************************
 802  *
 803  *  cpia2_usb_probe
 804  *
 805  *  Probe and initialize.
 806  *****************************************************************************/
 807 static int cpia2_usb_probe(struct usb_interface *intf,
 808                            const struct usb_device_id *id)
 809 {
 810         struct usb_device *udev = interface_to_usbdev(intf);
 811         struct usb_interface_descriptor *interface;
 812         struct camera_data *cam;
 813         int ret;
 814
 815         /* A multi-config CPiA2 camera? */
 816         if (udev->descriptor.bNumConfigurations != 1)
 817                 return -ENODEV;
 818         interface = &intf->cur_altsetting->desc;
 819
 820         /* If we get to this point, we found a CPiA2 camera */
 821         LOG("CPiA2 USB camera found\n");
 822
 823         cam = cpia2_init_camera_struct(intf);
 824         if (cam == NULL)
 825                 return -ENOMEM;
 826
 827         cam->dev = udev;
 828         cam->iface = interface->bInterfaceNumber;
 829
 830         ret = set_alternate(cam, USBIF_CMDONLY);
 831         if (ret < 0) {
 832                 ERR("%s: usb_set_interface error (ret = %d)\n", __func__, ret);
 833                 kfree(cam);
 834                 return ret;
 835         }
 836
 837
 838         if((ret = cpia2_init_camera(cam)) < 0) {
 839                 ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
 840                 kfree(cam);
 841                 return ret;
 842         }
 843         LOG("  CPiA Version: %d.%02d (%d.%d)\n",
 844                cam->params.version.firmware_revision_hi,
 845                cam->params.version.firmware_revision_lo,
 846                cam->params.version.asic_id,
 847                cam->params.version.asic_rev);
 848         LOG("  CPiA PnP-ID: %04x:%04x:%04x\n",
 849                cam->params.pnp_id.vendor,
 850                cam->params.pnp_id.product,
 851                cam->params.pnp_id.device_revision);
 852         LOG("  SensorID: %d.(version %d)\n",
 853                cam->params.version.sensor_flags,
 854                cam->params.version.sensor_rev);
 855
 856         usb_set_intfdata(intf, cam);
 857
 858         ret = cpia2_register_camera(cam);
 859         if (ret < 0) {
 860                 ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
 861                 kfree(cam);
 862                 return ret;
 863         }
 864
 865         return 0;
 866 }
 867
 868 /******************************************************************************
 869  *
 870  *  cpia2_disconnect
 871  *
 872  *****************************************************************************/
 873 static void cpia2_usb_disconnect(struct usb_interface *intf)
 874 {
 875         struct camera_data *cam = usb_get_intfdata(intf);
 876         usb_set_intfdata(intf, NULL);
 877
 878         DBG("Stopping stream\n");
 879         cpia2_usb_stream_stop(cam);
 880
 881         mutex_lock(&cam->v4l2_lock);
 882         DBG("Unregistering camera\n");
 883         cpia2_unregister_camera(cam);
 884         v4l2_device_disconnect(&cam->v4l2_dev);
 885         mutex_unlock(&cam->v4l2_lock);
 886         v4l2_device_put(&cam->v4l2_dev);
 887
 888         if(cam->buffers) {
 889                 DBG("Wakeup waiting processes\n");
 890                 cam->curbuff->status = FRAME_READY;
 891                 cam->curbuff->length = 0;
 892                 wake_up_interruptible(&cam->wq_stream);
 893         }
 894
 895         DBG("Releasing interface\n");
 896         usb_driver_release_interface(&cpia2_driver, intf);
 897
 898         LOG("CPiA2 camera disconnected.\n");
 899 }
 900
 901 static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message)
 902 {
 903         struct camera_data *cam = usb_get_intfdata(intf);
 904
 905         mutex_lock(&cam->v4l2_lock);
 906         if (cam->streaming) {
 907                 cpia2_usb_stream_stop(cam);
 908                 cam->streaming = 1;
 909         }
 910         mutex_unlock(&cam->v4l2_lock);
 911
 912         dev_info(&intf->dev, "going into suspend..\n");
 913         return 0;
 914 }
 915
 916 /* Resume device - start device. */
 917 static int cpia2_usb_resume(struct usb_interface *intf)
 918 {
 919         struct camera_data *cam = usb_get_intfdata(intf);
 920
 921         mutex_lock(&cam->v4l2_lock);
 922         v4l2_ctrl_handler_setup(&cam->hdl);
 923         if (cam->streaming) {
 924                 cam->streaming = 0;
 925                 cpia2_usb_stream_start(cam,
 926                                 cam->params.camera_state.stream_mode);
 927         }
 928         mutex_unlock(&cam->v4l2_lock);
 929
 930         dev_info(&intf->dev, "coming out of suspend..\n");
 931         return 0;
 932 }
 933
 934 /******************************************************************************
 935  *
 936  *  usb_cpia2_init
 937  *
 938  *****************************************************************************/
 939 int cpia2_usb_init(void)
 940 {
 941         return usb_register(&cpia2_driver);
 942 }
 943
 944 /******************************************************************************
 945  *
 946  *  usb_cpia_cleanup
 947  *
 948  *****************************************************************************/
 949 void cpia2_usb_cleanup(void)
 950 {
 951         schedule_timeout(2 * HZ);
 952         usb_deregister(&cpia2_driver);
 953 }