drivers/media/video/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
  58 static void free_sbufs(struct camera_data *cam);
  59 static void add_APPn(struct camera_data *cam);
  60 static void add_COM(struct camera_data *cam);
  61 static int submit_urbs(struct camera_data *cam);
  62 static int set_alternate(struct camera_data *cam, unsigned int alt);
  63 static int configure_transfer_mode(struct camera_data *cam, unsigned int alt);
  64
  65 static struct usb_device_id cpia2_id_table[] = {
  66         {USB_DEVICE(0x0553, 0x0100)},
  67         {USB_DEVICE(0x0553, 0x0140)},
  68         {USB_DEVICE(0x0553, 0x0151)},  /* STV0676 */
  69         {}                      /* Terminating entry */
  70 };
  71 MODULE_DEVICE_TABLE(usb, cpia2_id_table);
  72
  73 static struct usb_driver cpia2_driver = {
  74         .name           = "cpia2",
  75         .probe          = cpia2_usb_probe,
  76         .disconnect     = cpia2_usb_disconnect,
  77         .id_table       = cpia2_id_table
  78 };
  79
  80
  81 /******************************************************************************
  82  *
  83  *  process_frame
  84  *
  85  *****************************************************************************/
  86 static void process_frame(struct camera_data *cam)
  87 {
  88         static int frame_count;
  89
  90         unsigned char *inbuff = cam->workbuff->data;
  91
  92         DBG("Processing frame #%d, current:%d\n",
  93             cam->workbuff->num, cam->curbuff->num);
  94
  95         if(cam->workbuff->length > cam->workbuff->max_length)
  96                 cam->workbuff->max_length = cam->workbuff->length;
  97
  98         if ((inbuff[0] == 0xFF) && (inbuff[1] == 0xD8)) {
  99                 frame_count++;
 100         } else {
 101                 cam->workbuff->status = FRAME_ERROR;
 102                 DBG("Start of frame not found\n");
 103                 return;
 104         }
 105
 106         /***
 107          * Now the output buffer should have a JPEG image in it.
 108          ***/
 109         if(!cam->first_image_seen) {
 110                 /* Always skip the first image after streaming
 111                  * starts. It is almost certainly corrupt. */
 112                 cam->first_image_seen = 1;
 113                 cam->workbuff->status = FRAME_EMPTY;
 114                 return;
 115         }
 116         if (cam->workbuff->length > 3) {
 117                 if(cam->mmapped &&
 118                    cam->workbuff->length < cam->workbuff->max_length) {
 119                         /* No junk in the buffers */
 120                         memset(cam->workbuff->data+cam->workbuff->length,
 121                                0, cam->workbuff->max_length-
 122                                   cam->workbuff->length);
 123                 }
 124                 cam->workbuff->max_length = cam->workbuff->length;
 125                 cam->workbuff->status = FRAME_READY;
 126
 127                 if(!cam->mmapped && cam->num_frames > 2) {
 128                         /* During normal reading, the most recent
 129                          * frame will be read.  If the current frame
 130                          * hasn't started reading yet, it will never
 131                          * be read, so mark it empty.  If the buffer is
 132                          * mmapped, or we have few buffers, we need to
 133                          * wait for the user to free the buffer.
 134                          *
 135                          * NOTE: This is not entirely foolproof with 3
 136                          * buffers, but it would take an EXTREMELY
 137                          * overloaded system to cause problems (possible
 138                          * image data corruption).  Basically, it would
 139                          * need to take more time to execute cpia2_read
 140                          * than it would for the camera to send
 141                          * cam->num_frames-2 frames before problems
 142                          * could occur.
 143                          */
 144                         cam->curbuff->status = FRAME_EMPTY;
 145                 }
 146                 cam->curbuff = cam->workbuff;
 147                 cam->workbuff = cam->workbuff->next;
 148                 DBG("Changed buffers, work:%d, current:%d\n",
 149                     cam->workbuff->num, cam->curbuff->num);
 150                 return;
 151         } else {
 152                 DBG("Not enough data for an image.\n");
 153         }
 154
 155         cam->workbuff->status = FRAME_ERROR;
 156         return;
 157 }
 158
 159 /******************************************************************************
 160  *
 161  *  add_APPn
 162  *
 163  *  Adds a user specified APPn record
 164  *****************************************************************************/
 165 static void add_APPn(struct camera_data *cam)
 166 {
 167         if(cam->APP_len > 0) {
 168                 cam->workbuff->data[cam->workbuff->length++] = 0xFF;
 169                 cam->workbuff->data[cam->workbuff->length++] = 0xE0+cam->APPn;
 170                 cam->workbuff->data[cam->workbuff->length++] = 0;
 171                 cam->workbuff->data[cam->workbuff->length++] = cam->APP_len+2;
 172                 memcpy(cam->workbuff->data+cam->workbuff->length,
 173                        cam->APP_data, cam->APP_len);
 174                 cam->workbuff->length += cam->APP_len;
 175         }
 176 }
 177
 178 /******************************************************************************
 179  *
 180  *  add_COM
 181  *
 182  *  Adds a user specified COM record
 183  *****************************************************************************/
 184 static void add_COM(struct camera_data *cam)
 185 {
 186         if(cam->COM_len > 0) {
 187                 cam->workbuff->data[cam->workbuff->length++] = 0xFF;
 188                 cam->workbuff->data[cam->workbuff->length++] = 0xFE;
 189                 cam->workbuff->data[cam->workbuff->length++] = 0;
 190                 cam->workbuff->data[cam->workbuff->length++] = cam->COM_len+2;
 191                 memcpy(cam->workbuff->data+cam->workbuff->length,
 192                        cam->COM_data, cam->COM_len);
 193                 cam->workbuff->length += cam->COM_len;
 194         }
 195 }
 196
 197 /******************************************************************************
 198  *
 199  *  cpia2_usb_complete
 200  *
 201  *  callback when incoming packet is received
 202  *****************************************************************************/
 203 static void cpia2_usb_complete(struct urb *urb)
 204 {
 205         int i;
 206         unsigned char *cdata;
 207         static int frame_ready = false;
 208         struct camera_data *cam = (struct camera_data *) urb->context;
 209
 210         if (urb->status!=0) {
 211                 if (!(urb->status == -ENOENT ||
 212                       urb->status == -ECONNRESET ||
 213                       urb->status == -ESHUTDOWN))
 214                 {
 215                         DBG("urb->status = %d!\n", urb->status);
 216                 }
 217                 DBG("Stopping streaming\n");
 218                 return;
 219         }
 220
 221         if (!cam->streaming || !cam->present || cam->open_count == 0) {
 222                 LOG("Will now stop the streaming: streaming = %d, "
 223                     "present=%d, open_count=%d\n",
 224                     cam->streaming, cam->present, cam->open_count);
 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                         do_gettimeofday(&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(!cam->present)
 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         if (!registers || size <= 0)
 545                 return -EINVAL;
 546
 547         return usb_control_msg(udev,
 548                                usb_sndctrlpipe(udev, 0),
 549                                request,
 550                                USB_TYPE_VENDOR | USB_RECIP_DEVICE,
 551                                start,   /* value */
 552                                0,       /* index */
 553                                registers,       /* buffer */
 554                                size,
 555                                HZ);
 556 }
 557
 558 /****************************************************************************
 559  *
 560  *  read_packet
 561  *
 562  ***************************************************************************/
 563 static int read_packet(struct usb_device *udev,
 564                        u8 request, u8 * registers, u16 start, size_t size)
 565 {
 566         if (!registers || size <= 0)
 567                 return -EINVAL;
 568
 569         return usb_control_msg(udev,
 570                                usb_rcvctrlpipe(udev, 0),
 571                                request,
 572                                USB_DIR_IN|USB_TYPE_VENDOR|USB_RECIP_DEVICE,
 573                                start,   /* value */
 574                                0,       /* index */
 575                                registers,       /* buffer */
 576                                size,
 577                                HZ);
 578 }
 579
 580 /******************************************************************************
 581  *
 582  *  cpia2_usb_transfer_cmd
 583  *
 584  *****************************************************************************/
 585 int cpia2_usb_transfer_cmd(struct camera_data *cam,
 586                            void *registers,
 587                            u8 request, u8 start, u8 count, u8 direction)
 588 {
 589         int err = 0;
 590         struct usb_device *udev = cam->dev;
 591
 592         if (!udev) {
 593                 ERR("%s: Internal driver error: udev is NULL\n", __func__);
 594                 return -EINVAL;
 595         }
 596
 597         if (!registers) {
 598                 ERR("%s: Internal driver error: register array is NULL\n", __func__);
 599                 return -EINVAL;
 600         }
 601
 602         if (direction == TRANSFER_READ) {
 603                 err = read_packet(udev, request, (u8 *)registers, start, count);
 604                 if (err > 0)
 605                         err = 0;
 606         } else if (direction == TRANSFER_WRITE) {
 607                 err =write_packet(udev, request, (u8 *)registers, start, count);
 608                 if (err < 0) {
 609                         LOG("Control message failed, err val = %d\n", err);
 610                         LOG("Message: request = 0x%0X, start = 0x%0X\n",
 611                             request, start);
 612                         LOG("Message: count = %d, register[0] = 0x%0X\n",
 613                             count, ((unsigned char *) registers)[0]);
 614                 } else
 615                         err=0;
 616         } else {
 617                 LOG("Unexpected first byte of direction: %d\n",
 618                        direction);
 619                 return -EINVAL;
 620         }
 621
 622         if(err != 0)
 623                 LOG("Unexpected error: %d\n", err);
 624         return err;
 625 }
 626
 627
 628 /******************************************************************************
 629  *
 630  *  submit_urbs
 631  *
 632  *****************************************************************************/
 633 static int submit_urbs(struct camera_data *cam)
 634 {
 635         struct urb *urb;
 636         int fx, err, i, j;
 637
 638         for(i=0; i<NUM_SBUF; ++i) {
 639                 if (cam->sbuf[i].data)
 640                         continue;
 641                 cam->sbuf[i].data =
 642                     kmalloc(FRAMES_PER_DESC * FRAME_SIZE_PER_DESC, GFP_KERNEL);
 643                 if (!cam->sbuf[i].data) {
 644                         while (--i >= 0) {
 645                                 kfree(cam->sbuf[i].data);
 646                                 cam->sbuf[i].data = NULL;
 647                         }
 648                         return -ENOMEM;
 649                 }
 650         }
 651
 652         /* We double buffer the Isoc lists, and also know the polling
 653          * interval is every frame (1 == (1 << (bInterval -1))).
 654          */
 655         for(i=0; i<NUM_SBUF; ++i) {
 656                 if(cam->sbuf[i].urb) {
 657                         continue;
 658                 }
 659                 urb = usb_alloc_urb(FRAMES_PER_DESC, GFP_KERNEL);
 660                 if (!urb) {
 661                         ERR("%s: usb_alloc_urb error!\n", __func__);
 662                         for (j = 0; j < i; j++)
 663                                 usb_free_urb(cam->sbuf[j].urb);
 664                         return -ENOMEM;
 665                 }
 666
 667                 cam->sbuf[i].urb = urb;
 668                 urb->dev = cam->dev;
 669                 urb->context = cam;
 670                 urb->pipe = usb_rcvisocpipe(cam->dev, 1 /*ISOC endpoint*/);
 671                 urb->transfer_flags = URB_ISO_ASAP;
 672                 urb->transfer_buffer = cam->sbuf[i].data;
 673                 urb->complete = cpia2_usb_complete;
 674                 urb->number_of_packets = FRAMES_PER_DESC;
 675                 urb->interval = 1;
 676                 urb->transfer_buffer_length =
 677                         FRAME_SIZE_PER_DESC * FRAMES_PER_DESC;
 678
 679                 for (fx = 0; fx < FRAMES_PER_DESC; fx++) {
 680                         urb->iso_frame_desc[fx].offset =
 681                                 FRAME_SIZE_PER_DESC * fx;
 682                         urb->iso_frame_desc[fx].length = FRAME_SIZE_PER_DESC;
 683                 }
 684         }
 685
 686
 687         /* Queue the ISO urbs, and resubmit in the completion handler */
 688         for(i=0; i<NUM_SBUF; ++i) {
 689                 err = usb_submit_urb(cam->sbuf[i].urb, GFP_KERNEL);
 690                 if (err) {
 691                         ERR("usb_submit_urb[%d]() = %d\n", i, err);
 692                         return err;
 693                 }
 694         }
 695
 696         return 0;
 697 }
 698
 699 /******************************************************************************
 700  *
 701  *  cpia2_usb_stream_start
 702  *
 703  *****************************************************************************/
 704 int cpia2_usb_stream_start(struct camera_data *cam, unsigned int alternate)
 705 {
 706         int ret;
 707         int old_alt;
 708
 709         if(cam->streaming)
 710                 return 0;
 711
 712         if (cam->flush) {
 713                 int i;
 714                 DBG("Flushing buffers\n");
 715                 for(i=0; i<cam->num_frames; ++i) {
 716                         cam->buffers[i].status = FRAME_EMPTY;
 717                         cam->buffers[i].length = 0;
 718                 }
 719                 cam->curbuff = &cam->buffers[0];
 720                 cam->workbuff = cam->curbuff->next;
 721                 cam->flush = false;
 722         }
 723
 724         old_alt = cam->params.camera_state.stream_mode;
 725         cam->params.camera_state.stream_mode = 0;
 726         ret = cpia2_usb_change_streaming_alternate(cam, alternate);
 727         if (ret < 0) {
 728                 int ret2;
 729                 ERR("cpia2_usb_change_streaming_alternate() = %d!\n", ret);
 730                 cam->params.camera_state.stream_mode = old_alt;
 731                 ret2 = set_alternate(cam, USBIF_CMDONLY);
 732                 if (ret2 < 0) {
 733                         ERR("cpia2_usb_change_streaming_alternate(%d) =%d has already "
 734                             "failed. Then tried to call "
 735                             "set_alternate(USBIF_CMDONLY) = %d.\n",
 736                             alternate, ret, ret2);
 737                 }
 738         } else {
 739                 cam->frame_count = 0;
 740                 cam->streaming = 1;
 741                 ret = cpia2_usb_stream_resume(cam);
 742         }
 743         return ret;
 744 }
 745
 746 /******************************************************************************
 747  *
 748  *  cpia2_usb_stream_pause
 749  *
 750  *****************************************************************************/
 751 int cpia2_usb_stream_pause(struct camera_data *cam)
 752 {
 753         int ret = 0;
 754         if(cam->streaming) {
 755                 ret = set_alternate(cam, USBIF_CMDONLY);
 756                 free_sbufs(cam);
 757         }
 758         return ret;
 759 }
 760
 761 /******************************************************************************
 762  *
 763  *  cpia2_usb_stream_resume
 764  *
 765  *****************************************************************************/
 766 int cpia2_usb_stream_resume(struct camera_data *cam)
 767 {
 768         int ret = 0;
 769         if(cam->streaming) {
 770                 cam->first_image_seen = 0;
 771                 ret = set_alternate(cam, cam->params.camera_state.stream_mode);
 772                 if(ret == 0) {
 773                         ret = submit_urbs(cam);
 774                 }
 775         }
 776         return ret;
 777 }
 778
 779 /******************************************************************************
 780  *
 781  *  cpia2_usb_stream_stop
 782  *
 783  *****************************************************************************/
 784 int cpia2_usb_stream_stop(struct camera_data *cam)
 785 {
 786         int ret;
 787         ret = cpia2_usb_stream_pause(cam);
 788         cam->streaming = 0;
 789         configure_transfer_mode(cam, 0);
 790         return ret;
 791 }
 792
 793 /******************************************************************************
 794  *
 795  *  cpia2_usb_probe
 796  *
 797  *  Probe and initialize.
 798  *****************************************************************************/
 799 static int cpia2_usb_probe(struct usb_interface *intf,
 800                            const struct usb_device_id *id)
 801 {
 802         struct usb_device *udev = interface_to_usbdev(intf);
 803         struct usb_interface_descriptor *interface;
 804         struct camera_data *cam;
 805         int ret;
 806
 807         /* A multi-config CPiA2 camera? */
 808         if (udev->descriptor.bNumConfigurations != 1)
 809                 return -ENODEV;
 810         interface = &intf->cur_altsetting->desc;
 811
 812         /* If we get to this point, we found a CPiA2 camera */
 813         LOG("CPiA2 USB camera found\n");
 814
 815         if((cam = cpia2_init_camera_struct()) == NULL)
 816                 return -ENOMEM;
 817
 818         cam->dev = udev;
 819         cam->iface = interface->bInterfaceNumber;
 820
 821         ret = set_alternate(cam, USBIF_CMDONLY);
 822         if (ret < 0) {
 823                 ERR("%s: usb_set_interface error (ret = %d)\n", __func__, ret);
 824                 kfree(cam);
 825                 return ret;
 826         }
 827
 828         if ((ret = cpia2_register_camera(cam)) < 0) {
 829                 ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
 830                 kfree(cam);
 831                 return ret;
 832         }
 833
 834
 835         if((ret = cpia2_init_camera(cam)) < 0) {
 836                 ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
 837                 cpia2_unregister_camera(cam);
 838                 kfree(cam);
 839                 return ret;
 840         }
 841         LOG("  CPiA Version: %d.%02d (%d.%d)\n",
 842                cam->params.version.firmware_revision_hi,
 843                cam->params.version.firmware_revision_lo,
 844                cam->params.version.asic_id,
 845                cam->params.version.asic_rev);
 846         LOG("  CPiA PnP-ID: %04x:%04x:%04x\n",
 847                cam->params.pnp_id.vendor,
 848                cam->params.pnp_id.product,
 849                cam->params.pnp_id.device_revision);
 850         LOG("  SensorID: %d.(version %d)\n",
 851                cam->params.version.sensor_flags,
 852                cam->params.version.sensor_rev);
 853
 854         usb_set_intfdata(intf, cam);
 855
 856         return 0;
 857 }
 858
 859 /******************************************************************************
 860  *
 861  *  cpia2_disconnect
 862  *
 863  *****************************************************************************/
 864 static void cpia2_usb_disconnect(struct usb_interface *intf)
 865 {
 866         struct camera_data *cam = usb_get_intfdata(intf);
 867         usb_set_intfdata(intf, NULL);
 868         cam->present = 0;
 869
 870         DBG("Stopping stream\n");
 871         cpia2_usb_stream_stop(cam);
 872
 873         DBG("Unregistering camera\n");
 874         cpia2_unregister_camera(cam);
 875
 876         if(cam->buffers) {
 877                 DBG("Wakeup waiting processes\n");
 878                 cam->curbuff->status = FRAME_READY;
 879                 cam->curbuff->length = 0;
 880                 if (waitqueue_active(&cam->wq_stream))
 881                         wake_up_interruptible(&cam->wq_stream);
 882         }
 883
 884         DBG("Releasing interface\n");
 885         usb_driver_release_interface(&cpia2_driver, intf);
 886
 887         if (cam->open_count == 0) {
 888                 DBG("Freeing camera structure\n");
 889                 kfree(cam);
 890         }
 891
 892         LOG("CPiA2 camera disconnected.\n");
 893 }
 894
 895
 896 /******************************************************************************
 897  *
 898  *  usb_cpia2_init
 899  *
 900  *****************************************************************************/
 901 int cpia2_usb_init(void)
 902 {
 903         return usb_register(&cpia2_driver);
 904 }
 905
 906 /******************************************************************************
 907  *
 908  *  usb_cpia_cleanup
 909  *
 910  *****************************************************************************/
 911 void cpia2_usb_cleanup(void)
 912 {
 913         schedule_timeout(2 * HZ);
 914         usb_deregister(&cpia2_driver);
 915 }