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