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