linux/drivers/media/usb/cpia2/cpia2_usb.c
<<
>>
Prefs
   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
  25static 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
  39static void process_frame(struct camera_data *cam);
  40static void cpia2_usb_complete(struct urb *urb);
  41static int cpia2_usb_probe(struct usb_interface *intf,
  42                           const struct usb_device_id *id);
  43static void cpia2_usb_disconnect(struct usb_interface *intf);
  44static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message);
  45static int cpia2_usb_resume(struct usb_interface *intf);
  46
  47static void free_sbufs(struct camera_data *cam);
  48static void add_APPn(struct camera_data *cam);
  49static void add_COM(struct camera_data *cam);
  50static int submit_urbs(struct camera_data *cam);
  51static int set_alternate(struct camera_data *cam, unsigned int alt);
  52static int configure_transfer_mode(struct camera_data *cam, unsigned int alt);
  53
  54static 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};
  60MODULE_DEVICE_TABLE(usb, cpia2_id_table);
  61
  62static 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 *****************************************************************************/
  78static 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 *****************************************************************************/
 157static 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 *****************************************************************************/
 176static 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 *****************************************************************************/
 195static 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 *****************************************************************************/
 371static 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 *****************************************************************************/
 443int 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 *****************************************************************************/
 473static 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 *****************************************************************************/
 507static 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 ***************************************************************************/
 532static 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                               1000);
 554
 555        kfree(buf);
 556        return ret;
 557}
 558
 559/****************************************************************************
 560 *
 561 *  read_packet
 562 *
 563 ***************************************************************************/
 564static 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                               1000);
 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 *****************************************************************************/
 600int 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 *****************************************************************************/
 648static 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 *****************************************************************************/
 723int 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 *****************************************************************************/
 768int 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 *****************************************************************************/
 783int 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 *****************************************************************************/
 805int 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 *****************************************************************************/
 821static 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                goto alt_err;
 848        }
 849
 850
 851        if((ret = cpia2_init_camera(cam)) < 0) {
 852                ERR("%s: failed to initialize cpia2 camera (ret = %d)\n", __func__, ret);
 853                goto alt_err;
 854        }
 855        LOG("  CPiA Version: %d.%02d (%d.%d)\n",
 856               cam->params.version.firmware_revision_hi,
 857               cam->params.version.firmware_revision_lo,
 858               cam->params.version.asic_id,
 859               cam->params.version.asic_rev);
 860        LOG("  CPiA PnP-ID: %04x:%04x:%04x\n",
 861               cam->params.pnp_id.vendor,
 862               cam->params.pnp_id.product,
 863               cam->params.pnp_id.device_revision);
 864        LOG("  SensorID: %d.(version %d)\n",
 865               cam->params.version.sensor_flags,
 866               cam->params.version.sensor_rev);
 867
 868        usb_set_intfdata(intf, cam);
 869
 870        ret = cpia2_register_camera(cam);
 871        if (ret < 0) {
 872                ERR("%s: Failed to register cpia2 camera (ret = %d)\n", __func__, ret);
 873                goto alt_err;
 874        }
 875
 876        return 0;
 877
 878alt_err:
 879        cpia2_deinit_camera_struct(cam, intf);
 880        return ret;
 881}
 882
 883/******************************************************************************
 884 *
 885 *  cpia2_disconnect
 886 *
 887 *****************************************************************************/
 888static void cpia2_usb_disconnect(struct usb_interface *intf)
 889{
 890        struct camera_data *cam = usb_get_intfdata(intf);
 891        usb_set_intfdata(intf, NULL);
 892
 893        DBG("Stopping stream\n");
 894        cpia2_usb_stream_stop(cam);
 895
 896        mutex_lock(&cam->v4l2_lock);
 897        DBG("Unregistering camera\n");
 898        cpia2_unregister_camera(cam);
 899        v4l2_device_disconnect(&cam->v4l2_dev);
 900        mutex_unlock(&cam->v4l2_lock);
 901
 902        if(cam->buffers) {
 903                DBG("Wakeup waiting processes\n");
 904                cam->curbuff->status = FRAME_READY;
 905                cam->curbuff->length = 0;
 906                wake_up_interruptible(&cam->wq_stream);
 907        }
 908
 909        v4l2_device_put(&cam->v4l2_dev);
 910
 911        LOG("CPiA2 camera disconnected.\n");
 912}
 913
 914static int cpia2_usb_suspend(struct usb_interface *intf, pm_message_t message)
 915{
 916        struct camera_data *cam = usb_get_intfdata(intf);
 917
 918        mutex_lock(&cam->v4l2_lock);
 919        if (cam->streaming) {
 920                cpia2_usb_stream_stop(cam);
 921                cam->streaming = 1;
 922        }
 923        mutex_unlock(&cam->v4l2_lock);
 924
 925        dev_info(&intf->dev, "going into suspend..\n");
 926        return 0;
 927}
 928
 929/* Resume device - start device. */
 930static int cpia2_usb_resume(struct usb_interface *intf)
 931{
 932        struct camera_data *cam = usb_get_intfdata(intf);
 933
 934        mutex_lock(&cam->v4l2_lock);
 935        v4l2_ctrl_handler_setup(&cam->hdl);
 936        if (cam->streaming) {
 937                cam->streaming = 0;
 938                cpia2_usb_stream_start(cam,
 939                                cam->params.camera_state.stream_mode);
 940        }
 941        mutex_unlock(&cam->v4l2_lock);
 942
 943        dev_info(&intf->dev, "coming out of suspend..\n");
 944        return 0;
 945}
 946
 947/******************************************************************************
 948 *
 949 *  usb_cpia2_init
 950 *
 951 *****************************************************************************/
 952int cpia2_usb_init(void)
 953{
 954        return usb_register(&cpia2_driver);
 955}
 956
 957/******************************************************************************
 958 *
 959 *  usb_cpia_cleanup
 960 *
 961 *****************************************************************************/
 962void cpia2_usb_cleanup(void)
 963{
 964        schedule_timeout(2 * HZ);
 965        usb_deregister(&cpia2_driver);
 966}
 967