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