drivers/media/usb/cpia2/cpia2_usb.c

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