V4L/DVB (11396): cx88: avoid reprogramming every audio register on A2 stereo/mono...
[linux-2.6] / drivers / media / video / se401.c
1 /*
2  * Endpoints (formerly known as AOX) se401 USB Camera Driver
3  *
4  * Copyright (c) 2000 Jeroen B. Vreeken (pe1rxq@amsat.org)
5  *
6  * Still somewhat based on the Linux ov511 driver.
7  *
8  * This program is free software; you can redistribute it and/or modify it
9  * under the terms of the GNU General Public License as published by the
10  * Free Software Foundation; either version 2 of the License, or (at your
11  * option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
15  * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
16  * for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program; if not, write to the Free Software Foundation,
20  * Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
21  *
22  *
23  * Thanks to Endpoints Inc. (www.endpoints.com) for making documentation on
24  * their chipset available and supporting me while writing this driver.
25  *      - Jeroen Vreeken
26  */
27
28 static const char version[] = "0.24";
29
30 #include <linux/module.h>
31 #include <linux/init.h>
32 #include <linux/vmalloc.h>
33 #include <linux/slab.h>
34 #include <linux/pagemap.h>
35 #include <linux/usb.h>
36 #include "se401.h"
37
38 static int flickerless;
39 static int video_nr = -1;
40
41 static struct usb_device_id device_table[] = {
42         { USB_DEVICE(0x03e8, 0x0004) },/* Endpoints/Aox SE401 */
43         { USB_DEVICE(0x0471, 0x030b) },/* Philips PCVC665K */
44         { USB_DEVICE(0x047d, 0x5001) },/* Kensington 67014 */
45         { USB_DEVICE(0x047d, 0x5002) },/* Kensington 6701(5/7) */
46         { USB_DEVICE(0x047d, 0x5003) },/* Kensington 67016 */
47         { }
48 };
49
50 MODULE_DEVICE_TABLE(usb, device_table);
51
52 MODULE_AUTHOR("Jeroen Vreeken <pe1rxq@amsat.org>");
53 MODULE_DESCRIPTION("SE401 USB Camera Driver");
54 MODULE_LICENSE("GPL");
55 module_param(flickerless, int, 0);
56 MODULE_PARM_DESC(flickerless,
57                 "Net frequency to adjust exposure time to (0/50/60)");
58 module_param(video_nr, int, 0);
59
60 static struct usb_driver se401_driver;
61
62
63 /**********************************************************************
64  *
65  * Memory management
66  *
67  **********************************************************************/
68 static void *rvmalloc(unsigned long size)
69 {
70         void *mem;
71         unsigned long adr;
72
73         size = PAGE_ALIGN(size);
74         mem = vmalloc_32(size);
75         if (!mem)
76                 return NULL;
77
78         memset(mem, 0, size); /* Clear the ram out, no junk to the user */
79         adr = (unsigned long) mem;
80         while (size > 0) {
81                 SetPageReserved(vmalloc_to_page((void *)adr));
82                 adr +=  PAGE_SIZE;
83                 size -=  PAGE_SIZE;
84         }
85
86         return mem;
87 }
88
89 static void rvfree(void *mem, unsigned long size)
90 {
91         unsigned long adr;
92
93         if (!mem)
94                 return;
95
96         adr = (unsigned long) mem;
97         while ((long) size > 0) {
98                 ClearPageReserved(vmalloc_to_page((void *)adr));
99                 adr +=  PAGE_SIZE;
100                 size -=  PAGE_SIZE;
101         }
102         vfree(mem);
103 }
104
105
106
107 /****************************************************************************
108  *
109  * se401 register read/write functions
110  *
111  ***************************************************************************/
112
113 static int se401_sndctrl(int set, struct usb_se401 *se401, unsigned short req,
114                          unsigned short value, unsigned char *cp, int size)
115 {
116         return usb_control_msg(
117                 se401->dev,
118                 set ? usb_sndctrlpipe(se401->dev, 0) : usb_rcvctrlpipe(se401->dev, 0),
119                 req,
120                 (set ? USB_DIR_OUT : USB_DIR_IN) | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
121                 value,
122                 0,
123                 cp,
124                 size,
125                 1000
126         );
127 }
128
129 static int se401_set_feature(struct usb_se401 *se401, unsigned short selector,
130                              unsigned short param)
131 {
132         /* specs say that the selector (address) should go in the value field
133            and the param in index, but in the logs of the windows driver they do
134            this the other way around...
135          */
136         return usb_control_msg(
137                 se401->dev,
138                 usb_sndctrlpipe(se401->dev, 0),
139                 SE401_REQ_SET_EXT_FEATURE,
140                 USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
141                 param,
142                 selector,
143                 NULL,
144                 0,
145                 1000
146         );
147 }
148
149 static unsigned short se401_get_feature(struct usb_se401 *se401,
150                                         unsigned short selector)
151 {
152         /* For 'set' the selecetor should be in index, not sure if the spec is
153            wrong here to....
154          */
155         unsigned char cp[2];
156         usb_control_msg(
157                 se401->dev,
158                 usb_rcvctrlpipe(se401->dev, 0),
159                 SE401_REQ_GET_EXT_FEATURE,
160                 USB_DIR_OUT | USB_TYPE_VENDOR | USB_RECIP_DEVICE,
161                 0,
162                 selector,
163                 cp,
164                 2,
165                 1000
166         );
167         return cp[0]+cp[1]*256;
168 }
169
170 /****************************************************************************
171  *
172  * Camera control
173  *
174  ***************************************************************************/
175
176
177 static int se401_send_pict(struct usb_se401 *se401)
178 {
179         /* integration time low */
180         se401_set_feature(se401, HV7131_REG_TITL, se401->expose_l);
181         /* integration time mid */
182         se401_set_feature(se401, HV7131_REG_TITM, se401->expose_m);
183         /* integration time mid */
184         se401_set_feature(se401, HV7131_REG_TITU, se401->expose_h);
185         /* reset level value */
186         se401_set_feature(se401, HV7131_REG_ARLV, se401->resetlevel);
187         /* red color gain */
188         se401_set_feature(se401, HV7131_REG_ARCG, se401->rgain);
189         /* green color gain */
190         se401_set_feature(se401, HV7131_REG_AGCG, se401->ggain);
191         /* blue color gain */
192         se401_set_feature(se401, HV7131_REG_ABCG, se401->bgain);
193
194         return 0;
195 }
196
197 static void se401_set_exposure(struct usb_se401 *se401, int brightness)
198 {
199         int integration = brightness << 5;
200
201         if (flickerless == 50)
202                 integration = integration-integration % 106667;
203         if (flickerless == 60)
204                 integration = integration-integration % 88889;
205         se401->brightness = integration >> 5;
206         se401->expose_h = (integration >> 16) & 0xff;
207         se401->expose_m = (integration >> 8) & 0xff;
208         se401->expose_l = integration & 0xff;
209 }
210
211 static int se401_get_pict(struct usb_se401 *se401, struct video_picture *p)
212 {
213         p->brightness = se401->brightness;
214         if (se401->enhance)
215                 p->whiteness = 32768;
216         else
217                 p->whiteness = 0;
218
219         p->colour = 65535;
220         p->contrast = 65535;
221         p->hue = se401->rgain << 10;
222         p->palette = se401->palette;
223         p->depth = 3; /* rgb24 */
224         return 0;
225 }
226
227
228 static int se401_set_pict(struct usb_se401 *se401, struct video_picture *p)
229 {
230         if (p->palette != VIDEO_PALETTE_RGB24)
231                 return 1;
232         se401->palette = p->palette;
233         if (p->hue != se401->hue) {
234                 se401->rgain =  p->hue >> 10;
235                 se401->bgain =  0x40-(p->hue >> 10);
236                 se401->hue = p->hue;
237         }
238         if (p->brightness != se401->brightness)
239                 se401_set_exposure(se401, p->brightness);
240
241         if (p->whiteness >= 32768)
242                 se401->enhance = 1;
243         else
244                 se401->enhance = 0;
245         se401_send_pict(se401);
246         se401_send_pict(se401);
247         return 0;
248 }
249
250 /*
251         Hyundai have some really nice docs about this and other sensor related
252         stuff on their homepage: www.hei.co.kr
253 */
254 static void se401_auto_resetlevel(struct usb_se401 *se401)
255 {
256         unsigned int ahrc, alrc;
257         int oldreset = se401->resetlevel;
258
259         /* For some reason this normally read-only register doesn't get reset
260            to zero after reading them just once...
261          */
262         se401_get_feature(se401, HV7131_REG_HIREFNOH);
263         se401_get_feature(se401, HV7131_REG_HIREFNOL);
264         se401_get_feature(se401, HV7131_REG_LOREFNOH);
265         se401_get_feature(se401, HV7131_REG_LOREFNOL);
266         ahrc = 256*se401_get_feature(se401, HV7131_REG_HIREFNOH) +
267             se401_get_feature(se401, HV7131_REG_HIREFNOL);
268         alrc = 256*se401_get_feature(se401, HV7131_REG_LOREFNOH) +
269             se401_get_feature(se401, HV7131_REG_LOREFNOL);
270
271         /* Not an exact science, but it seems to work pretty well... */
272         if (alrc > 10) {
273                 while (alrc >= 10 && se401->resetlevel < 63) {
274                         se401->resetlevel++;
275                         alrc /= 2;
276                 }
277         } else if (ahrc > 20) {
278                 while (ahrc >= 20 && se401->resetlevel > 0) {
279                         se401->resetlevel--;
280                         ahrc /= 2;
281                 }
282         }
283         if (se401->resetlevel != oldreset)
284                 se401_set_feature(se401, HV7131_REG_ARLV, se401->resetlevel);
285
286         return;
287 }
288
289 /* irq handler for snapshot button */
290 static void se401_button_irq(struct urb *urb)
291 {
292         struct usb_se401 *se401 = urb->context;
293         int status;
294
295         if (!se401->dev) {
296                 dev_info(&urb->dev->dev, "device vapourished\n");
297                 return;
298         }
299
300         switch (urb->status) {
301         case 0:
302                 /* success */
303                 break;
304         case -ECONNRESET:
305         case -ENOENT:
306         case -ESHUTDOWN:
307                 /* this urb is terminated, clean up */
308                 dbg("%s - urb shutting down with status: %d",
309                                                         __func__, urb->status);
310                 return;
311         default:
312                 dbg("%s - nonzero urb status received: %d",
313                                                         __func__, urb->status);
314                 goto exit;
315         }
316
317         if (urb->actual_length  >= 2)
318                 if (se401->button)
319                         se401->buttonpressed = 1;
320 exit:
321         status = usb_submit_urb(urb, GFP_ATOMIC);
322         if (status)
323                 err("%s - usb_submit_urb failed with result %d",
324                      __func__, status);
325 }
326
327 static void se401_video_irq(struct urb *urb)
328 {
329         struct usb_se401 *se401 = urb->context;
330         int length = urb->actual_length;
331
332         /* ohoh... */
333         if (!se401->streaming)
334                 return;
335
336         if (!se401->dev) {
337                 dev_info(&urb->dev->dev, "device vapourished\n");
338                 return;
339         }
340
341         /* 0 sized packets happen if we are to fast, but sometimes the camera
342            keeps sending them forever...
343          */
344         if (length && !urb->status) {
345                 se401->nullpackets = 0;
346                 switch (se401->scratch[se401->scratch_next].state) {
347                 case BUFFER_READY:
348                 case BUFFER_BUSY:
349                         se401->dropped++;
350                         break;
351                 case BUFFER_UNUSED:
352                         memcpy(se401->scratch[se401->scratch_next].data,
353                                 (unsigned char *)urb->transfer_buffer, length);
354                         se401->scratch[se401->scratch_next].state
355                                                         = BUFFER_READY;
356                         se401->scratch[se401->scratch_next].offset
357                                                         = se401->bayeroffset;
358                         se401->scratch[se401->scratch_next].length = length;
359                         if (waitqueue_active(&se401->wq))
360                                 wake_up_interruptible(&se401->wq);
361                         se401->scratch_overflow = 0;
362                         se401->scratch_next++;
363                         if (se401->scratch_next >= SE401_NUMSCRATCH)
364                                 se401->scratch_next = 0;
365                         break;
366                 }
367                 se401->bayeroffset += length;
368                 if (se401->bayeroffset >= se401->cheight * se401->cwidth)
369                         se401->bayeroffset = 0;
370         } else {
371                 se401->nullpackets++;
372                 if (se401->nullpackets > SE401_MAX_NULLPACKETS)
373                         if (waitqueue_active(&se401->wq))
374                                 wake_up_interruptible(&se401->wq);
375         }
376
377         /* Resubmit urb for new data */
378         urb->status = 0;
379         urb->dev = se401->dev;
380         if (usb_submit_urb(urb, GFP_KERNEL))
381                 dev_info(&urb->dev->dev, "urb burned down\n");
382         return;
383 }
384
385 static void se401_send_size(struct usb_se401 *se401, int width, int height)
386 {
387         int i = 0;
388         int mode = 0x03; /* No compression */
389         int sendheight = height;
390         int sendwidth = width;
391
392         /* JangGu compression can only be used with the camera supported sizes,
393            but bayer seems to work with any size that fits on the sensor.
394            We check if we can use compression with the current size with either
395            4 or 16 times subcapturing, if not we use uncompressed bayer data
396            but this will result in cutouts of the maximum size....
397          */
398         while (i < se401->sizes && !(se401->width[i] == width &&
399                                                 se401->height[i] == height))
400                 i++;
401         while (i < se401->sizes) {
402                 if (se401->width[i] == width * 2 &&
403                                 se401->height[i] == height * 2) {
404                         sendheight = se401->height[i];
405                         sendwidth = se401->width[i];
406                         mode = 0x40;
407                 }
408                 if (se401->width[i] == width * 4 &&
409                                 se401->height[i] == height * 4) {
410                         sendheight = se401->height[i];
411                         sendwidth = se401->width[i];
412                         mode = 0x42;
413                 }
414                 i++;
415         }
416
417         se401_sndctrl(1, se401, SE401_REQ_SET_WIDTH, sendwidth, NULL, 0);
418         se401_sndctrl(1, se401, SE401_REQ_SET_HEIGHT, sendheight, NULL, 0);
419         se401_set_feature(se401, SE401_OPERATINGMODE, mode);
420
421         if (mode == 0x03)
422                 se401->format = FMT_BAYER;
423         else
424                 se401->format = FMT_JANGGU;
425 }
426
427 /*
428         In this function se401_send_pict is called several times,
429         for some reason (depending on the state of the sensor and the phase of
430         the moon :) doing this only in either place doesn't always work...
431 */
432 static int se401_start_stream(struct usb_se401 *se401)
433 {
434         struct urb *urb;
435         int err = 0, i;
436         se401->streaming = 1;
437
438         se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 1, NULL, 0);
439         se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 1, NULL, 0);
440
441         /* Set picture settings */
442         /* windowed + pix intg */
443         se401_set_feature(se401, HV7131_REG_MODE_B, 0x05);
444         se401_send_pict(se401);
445
446         se401_send_size(se401, se401->cwidth, se401->cheight);
447
448         se401_sndctrl(1, se401, SE401_REQ_START_CONTINUOUS_CAPTURE,
449                                                                 0, NULL, 0);
450
451         /* Do some memory allocation */
452         for (i = 0; i < SE401_NUMFRAMES; i++) {
453                 se401->frame[i].data = se401->fbuf + i * se401->maxframesize;
454                 se401->frame[i].curpix = 0;
455         }
456         for (i = 0; i < SE401_NUMSBUF; i++) {
457                 se401->sbuf[i].data = kmalloc(SE401_PACKETSIZE, GFP_KERNEL);
458                 if (!se401->sbuf[i].data) {
459                         for (i = i - 1; i >= 0; i--) {
460                                 kfree(se401->sbuf[i].data);
461                                 se401->sbuf[i].data = NULL;
462                         }
463                         return -ENOMEM;
464                 }
465         }
466
467         se401->bayeroffset = 0;
468         se401->scratch_next = 0;
469         se401->scratch_use = 0;
470         se401->scratch_overflow = 0;
471         for (i = 0; i < SE401_NUMSCRATCH; i++) {
472                 se401->scratch[i].data = kmalloc(SE401_PACKETSIZE, GFP_KERNEL);
473                 if (!se401->scratch[i].data) {
474                         for (i = i - 1; i >= 0; i--) {
475                                 kfree(se401->scratch[i].data);
476                                 se401->scratch[i].data = NULL;
477                         }
478                         goto nomem_sbuf;
479                 }
480                 se401->scratch[i].state = BUFFER_UNUSED;
481         }
482
483         for (i = 0; i < SE401_NUMSBUF; i++) {
484                 urb = usb_alloc_urb(0, GFP_KERNEL);
485                 if (!urb) {
486                         for (i = i - 1; i >= 0; i--) {
487                                 usb_kill_urb(se401->urb[i]);
488                                 usb_free_urb(se401->urb[i]);
489                                 se401->urb[i] = NULL;
490                         }
491                         goto nomem_scratch;
492                 }
493
494                 usb_fill_bulk_urb(urb, se401->dev,
495                         usb_rcvbulkpipe(se401->dev, SE401_VIDEO_ENDPOINT),
496                         se401->sbuf[i].data, SE401_PACKETSIZE,
497                         se401_video_irq,
498                         se401);
499
500                 se401->urb[i] = urb;
501
502                 err = usb_submit_urb(se401->urb[i], GFP_KERNEL);
503                 if (err)
504                         err("urb burned down");
505         }
506
507         se401->framecount = 0;
508
509         return 0;
510
511  nomem_scratch:
512         for (i = 0; i < SE401_NUMSCRATCH; i++) {
513                 kfree(se401->scratch[i].data);
514                 se401->scratch[i].data = NULL;
515         }
516  nomem_sbuf:
517         for (i = 0; i < SE401_NUMSBUF; i++) {
518                 kfree(se401->sbuf[i].data);
519                 se401->sbuf[i].data = NULL;
520         }
521         return -ENOMEM;
522 }
523
524 static int se401_stop_stream(struct usb_se401 *se401)
525 {
526         int i;
527
528         if (!se401->streaming || !se401->dev)
529                 return 1;
530
531         se401->streaming = 0;
532
533         se401_sndctrl(1, se401, SE401_REQ_STOP_CONTINUOUS_CAPTURE, 0, NULL, 0);
534
535         se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 0, NULL, 0);
536         se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 0, NULL, 0);
537
538         for (i = 0; i < SE401_NUMSBUF; i++)
539                 if (se401->urb[i]) {
540                         usb_kill_urb(se401->urb[i]);
541                         usb_free_urb(se401->urb[i]);
542                         se401->urb[i] = NULL;
543                         kfree(se401->sbuf[i].data);
544                 }
545         for (i = 0; i < SE401_NUMSCRATCH; i++) {
546                 kfree(se401->scratch[i].data);
547                 se401->scratch[i].data = NULL;
548         }
549
550         return 0;
551 }
552
553 static int se401_set_size(struct usb_se401 *se401, int width, int height)
554 {
555         int wasstreaming = se401->streaming;
556         /* Check to see if we need to change */
557         if (se401->cwidth == width && se401->cheight == height)
558                 return 0;
559
560         /* Check for a valid mode */
561         if (!width || !height)
562                 return 1;
563         if ((width & 1) || (height & 1))
564                 return 1;
565         if (width > se401->width[se401->sizes-1])
566                 return 1;
567         if (height > se401->height[se401->sizes-1])
568                 return 1;
569
570         /* Stop a current stream and start it again at the new size */
571         if (wasstreaming)
572                 se401_stop_stream(se401);
573         se401->cwidth = width;
574         se401->cheight = height;
575         if (wasstreaming)
576                 se401_start_stream(se401);
577         return 0;
578 }
579
580
581 /****************************************************************************
582  *
583  * Video Decoding
584  *
585  ***************************************************************************/
586
587 /*
588         This shouldn't really be done in a v4l driver....
589         But it does make the image look a lot more usable.
590         Basically it lifts the dark pixels more than the light pixels.
591 */
592 static inline void enhance_picture(unsigned char *frame, int len)
593 {
594         while (len--) {
595                 *frame = (((*frame^255)*(*frame^255))/255)^255;
596                 frame++;
597         }
598 }
599
600 static inline void decode_JangGu_integrate(struct usb_se401 *se401, int data)
601 {
602         struct se401_frame *frame = &se401->frame[se401->curframe];
603         int linelength = se401->cwidth * 3;
604
605         if (frame->curlinepix >= linelength) {
606                 frame->curlinepix = 0;
607                 frame->curline += linelength;
608         }
609
610         /* First three are absolute, all others relative.
611          * Format is rgb from right to left (mirrorred image),
612          * we flip it to get bgr from left to right. */
613         if (frame->curlinepix < 3)
614                 *(frame->curline-frame->curlinepix) = 1 + data * 4;
615         else
616                 *(frame->curline-frame->curlinepix) =
617                     *(frame->curline-frame->curlinepix + 3) + data * 4;
618         frame->curlinepix++;
619 }
620
621 static inline void decode_JangGu_vlc(struct usb_se401 *se401,
622                         unsigned char *data, int bit_exp, int packetlength)
623 {
624         int pos = 0;
625         int vlc_cod = 0;
626         int vlc_size = 0;
627         int vlc_data = 0;
628         int bit_cur;
629         int bit;
630         data += 4;
631         while (pos < packetlength) {
632                 bit_cur = 8;
633                 while (bit_cur && bit_exp) {
634                         bit = ((*data) >> (bit_cur-1))&1;
635                         if (!vlc_cod) {
636                                 if (bit) {
637                                         vlc_size++;
638                                 } else {
639                                         if (!vlc_size)
640                                                 decode_JangGu_integrate(se401, 0);
641                                         else {
642                                                 vlc_cod = 2;
643                                                 vlc_data = 0;
644                                         }
645                                 }
646                         } else {
647                                 if (vlc_cod == 2) {
648                                         if (!bit)
649                                                 vlc_data =  -(1 << vlc_size) + 1;
650                                         vlc_cod--;
651                                 }
652                                 vlc_size--;
653                                 vlc_data += bit << vlc_size;
654                                 if (!vlc_size) {
655                                         decode_JangGu_integrate(se401, vlc_data);
656                                         vlc_cod = 0;
657                                 }
658                         }
659                         bit_cur--;
660                         bit_exp--;
661                 }
662                 pos++;
663                 data++;
664         }
665 }
666
667 static inline void decode_JangGu(struct usb_se401 *se401,
668                                                 struct se401_scratch *buffer)
669 {
670         unsigned char *data = buffer->data;
671         int len = buffer->length;
672         int bit_exp = 0, pix_exp = 0, frameinfo = 0, packetlength = 0, size;
673         int datapos = 0;
674
675         /* New image? */
676         if (!se401->frame[se401->curframe].curpix) {
677                 se401->frame[se401->curframe].curlinepix = 0;
678                 se401->frame[se401->curframe].curline =
679                     se401->frame[se401->curframe].data+
680                     se401->cwidth * 3 - 1;
681                 if (se401->frame[se401->curframe].grabstate == FRAME_READY)
682                         se401->frame[se401->curframe].grabstate = FRAME_GRABBING;
683                 se401->vlcdatapos = 0;
684         }
685         while (datapos < len) {
686                 size = 1024 - se401->vlcdatapos;
687                 if (size+datapos > len)
688                         size = len-datapos;
689                 memcpy(se401->vlcdata+se401->vlcdatapos, data+datapos, size);
690                 se401->vlcdatapos += size;
691                 packetlength = 0;
692                 if (se401->vlcdatapos >= 4) {
693                         bit_exp = se401->vlcdata[3] + (se401->vlcdata[2] << 8);
694                         pix_exp = se401->vlcdata[1] +
695                                         ((se401->vlcdata[0] & 0x3f) << 8);
696                         frameinfo = se401->vlcdata[0] & 0xc0;
697                         packetlength = ((bit_exp + 47) >> 4) << 1;
698                         if (packetlength > 1024) {
699                                 se401->vlcdatapos = 0;
700                                 datapos = len;
701                                 packetlength = 0;
702                                 se401->error++;
703                                 se401->frame[se401->curframe].curpix = 0;
704                         }
705                 }
706                 if (packetlength && se401->vlcdatapos >= packetlength) {
707                         decode_JangGu_vlc(se401, se401->vlcdata, bit_exp,
708                                                                 packetlength);
709                         se401->frame[se401->curframe].curpix += pix_exp * 3;
710                         datapos += size-(se401->vlcdatapos-packetlength);
711                         se401->vlcdatapos = 0;
712                         if (se401->frame[se401->curframe].curpix >= se401->cwidth * se401->cheight * 3) {
713                                 if (se401->frame[se401->curframe].curpix == se401->cwidth * se401->cheight * 3) {
714                                         if (se401->frame[se401->curframe].grabstate == FRAME_GRABBING) {
715                                                 se401->frame[se401->curframe].grabstate = FRAME_DONE;
716                                                 se401->framecount++;
717                                                 se401->readcount++;
718                                         }
719                                         if (se401->frame[(se401->curframe + 1) & (SE401_NUMFRAMES - 1)].grabstate == FRAME_READY)
720                                                 se401->curframe = (se401->curframe + 1) & (SE401_NUMFRAMES - 1);
721                                 } else
722                                         se401->error++;
723                                 se401->frame[se401->curframe].curpix = 0;
724                                 datapos = len;
725                         }
726                 } else
727                         datapos += size;
728         }
729 }
730
731 static inline void decode_bayer(struct usb_se401 *se401,
732                                                 struct se401_scratch *buffer)
733 {
734         unsigned char *data = buffer->data;
735         int len = buffer->length;
736         int offset = buffer->offset;
737         int datasize = se401->cwidth * se401->cheight;
738         struct se401_frame *frame = &se401->frame[se401->curframe];
739         unsigned char *framedata = frame->data, *curline, *nextline;
740         int width = se401->cwidth;
741         int blineoffset = 0, bline;
742         int linelength = width * 3, i;
743
744
745         if (frame->curpix == 0) {
746                 if (frame->grabstate == FRAME_READY)
747                         frame->grabstate = FRAME_GRABBING;
748
749                 frame->curline = framedata + linelength;
750                 frame->curlinepix = 0;
751         }
752
753         if (offset != frame->curpix) {
754                 /* Regard frame as lost :( */
755                 frame->curpix = 0;
756                 se401->error++;
757                 return;
758         }
759
760         /* Check if we have to much data */
761         if (frame->curpix + len > datasize)
762                 len = datasize-frame->curpix;
763
764         if (se401->cheight % 4)
765                 blineoffset = 1;
766         bline = frame->curpix / se401->cwidth+blineoffset;
767
768         curline = frame->curline;
769         nextline = curline + linelength;
770         if (nextline >= framedata+datasize * 3)
771                 nextline = curline;
772         while (len) {
773                 if (frame->curlinepix >= width) {
774                         frame->curlinepix -= width;
775                         bline = frame->curpix / width + blineoffset;
776                         curline += linelength*2;
777                         nextline += linelength*2;
778                         if (curline >= framedata+datasize * 3) {
779                                 frame->curlinepix++;
780                                 curline -= 3;
781                                 nextline -= 3;
782                                 len--;
783                                 data++;
784                                 frame->curpix++;
785                         }
786                         if (nextline >= framedata+datasize*3)
787                                 nextline = curline;
788                 }
789                 if (bline & 1) {
790                         if (frame->curlinepix & 1) {
791                                 *(curline + 2) = *data;
792                                 *(curline - 1) = *data;
793                                 *(nextline + 2) = *data;
794                                 *(nextline - 1) = *data;
795                         } else {
796                                 *(curline + 1) =
797                                         (*(curline + 1) + *data) / 2;
798                                 *(curline-2) =
799                                         (*(curline - 2) + *data) / 2;
800                                 *(nextline + 1) = *data;
801                                 *(nextline - 2) = *data;
802                         }
803                 } else {
804                         if (frame->curlinepix & 1) {
805                                 *(curline + 1) =
806                                         (*(curline + 1) + *data) / 2;
807                                 *(curline - 2) =
808                                         (*(curline - 2) + *data) / 2;
809                                 *(nextline + 1) = *data;
810                                 *(nextline - 2) = *data;
811                         } else {
812                                 *curline = *data;
813                                 *(curline - 3) = *data;
814                                 *nextline = *data;
815                                 *(nextline - 3) = *data;
816                         }
817                 }
818                 frame->curlinepix++;
819                 curline -= 3;
820                 nextline -= 3;
821                 len--;
822                 data++;
823                 frame->curpix++;
824         }
825         frame->curline = curline;
826
827         if (frame->curpix >= datasize) {
828                 /* Fix the top line */
829                 framedata += linelength;
830                 for (i = 0; i < linelength; i++) {
831                         framedata--;
832                         *framedata = *(framedata + linelength);
833                 }
834                 /* Fix the left side (green is already present) */
835                 for (i = 0; i < se401->cheight; i++) {
836                         *framedata = *(framedata + 3);
837                         *(framedata + 1) = *(framedata + 4);
838                         *(framedata + 2) = *(framedata + 5);
839                         framedata += linelength;
840                 }
841                 frame->curpix = 0;
842                 frame->grabstate = FRAME_DONE;
843                 se401->framecount++;
844                 se401->readcount++;
845                 if (se401->frame[(se401->curframe + 1) &
846                     (SE401_NUMFRAMES - 1)].grabstate == FRAME_READY) {
847                         se401->curframe = (se401->curframe+1) &
848                                                         (SE401_NUMFRAMES-1);
849                 }
850         }
851 }
852
853 static int se401_newframe(struct usb_se401 *se401, int framenr)
854 {
855         DECLARE_WAITQUEUE(wait, current);
856         int errors = 0;
857
858         while (se401->streaming &&
859             (se401->frame[framenr].grabstate == FRAME_READY ||
860              se401->frame[framenr].grabstate == FRAME_GRABBING)) {
861                 if (!se401->frame[framenr].curpix)
862                         errors++;
863
864                 wait_interruptible(
865                     se401->scratch[se401->scratch_use].state != BUFFER_READY,
866                                                     &se401->wq, &wait);
867                 if (se401->nullpackets > SE401_MAX_NULLPACKETS) {
868                         se401->nullpackets = 0;
869                         dev_info(&se401->dev->dev,
870                          "too many null length packets, restarting capture\n");
871                         se401_stop_stream(se401);
872                         se401_start_stream(se401);
873                 } else {
874                         if (se401->scratch[se401->scratch_use].state !=
875                                                                 BUFFER_READY) {
876                                 se401->frame[framenr].grabstate = FRAME_ERROR;
877                                 return -EIO;
878                         }
879                         se401->scratch[se401->scratch_use].state = BUFFER_BUSY;
880                         if (se401->format == FMT_JANGGU)
881                                 decode_JangGu(se401,
882                                         &se401->scratch[se401->scratch_use]);
883                         else
884                                 decode_bayer(se401,
885                                         &se401->scratch[se401->scratch_use]);
886
887                         se401->scratch[se401->scratch_use].state =
888                                                         BUFFER_UNUSED;
889                         se401->scratch_use++;
890                         if (se401->scratch_use >= SE401_NUMSCRATCH)
891                                 se401->scratch_use = 0;
892                         if (errors > SE401_MAX_ERRORS) {
893                                 errors = 0;
894                                 dev_info(&se401->dev->dev,
895                                       "too many errors, restarting capture\n");
896                                 se401_stop_stream(se401);
897                                 se401_start_stream(se401);
898                         }
899                 }
900         }
901
902         if (se401->frame[framenr].grabstate == FRAME_DONE)
903                 if (se401->enhance)
904                         enhance_picture(se401->frame[framenr].data,
905                                         se401->cheight * se401->cwidth * 3);
906         return 0;
907 }
908
909 static void usb_se401_remove_disconnected(struct usb_se401 *se401)
910 {
911         int i;
912
913         se401->dev = NULL;
914
915         for (i = 0; i < SE401_NUMSBUF; i++)
916                 if (se401->urb[i]) {
917                         usb_kill_urb(se401->urb[i]);
918                         usb_free_urb(se401->urb[i]);
919                         se401->urb[i] = NULL;
920                         kfree(se401->sbuf[i].data);
921                 }
922
923         for (i = 0; i < SE401_NUMSCRATCH; i++)
924                 kfree(se401->scratch[i].data);
925
926         if (se401->inturb) {
927                 usb_kill_urb(se401->inturb);
928                 usb_free_urb(se401->inturb);
929         }
930         dev_info(&se401->dev->dev, "%s disconnected", se401->camera_name);
931
932         /* Free the memory */
933         kfree(se401->width);
934         kfree(se401->height);
935         kfree(se401);
936 }
937
938
939
940 /****************************************************************************
941  *
942  * Video4Linux
943  *
944  ***************************************************************************/
945
946
947 static int se401_open(struct file *file)
948 {
949         struct video_device *dev = video_devdata(file);
950         struct usb_se401 *se401 = (struct usb_se401 *)dev;
951         int err = 0;
952
953         lock_kernel();
954         if (se401->user) {
955                 unlock_kernel();
956                 return -EBUSY;
957         }
958         se401->fbuf = rvmalloc(se401->maxframesize * SE401_NUMFRAMES);
959         if (se401->fbuf)
960                 file->private_data = dev;
961         else
962                 err = -ENOMEM;
963         se401->user = !err;
964         unlock_kernel();
965
966         return err;
967 }
968
969 static int se401_close(struct file *file)
970 {
971         struct video_device *dev = file->private_data;
972         struct usb_se401 *se401 = (struct usb_se401 *)dev;
973         int i;
974
975         rvfree(se401->fbuf, se401->maxframesize * SE401_NUMFRAMES);
976         if (se401->removed) {
977                 dev_info(&se401->dev->dev, "device unregistered\n");
978                 usb_se401_remove_disconnected(se401);
979         } else {
980                 for (i = 0; i < SE401_NUMFRAMES; i++)
981                         se401->frame[i].grabstate = FRAME_UNUSED;
982                 if (se401->streaming)
983                         se401_stop_stream(se401);
984                 se401->user = 0;
985         }
986         file->private_data = NULL;
987         return 0;
988 }
989
990 static long se401_do_ioctl(struct file *file, unsigned int cmd, void *arg)
991 {
992         struct video_device *vdev = file->private_data;
993         struct usb_se401 *se401 = (struct usb_se401 *)vdev;
994
995         if (!se401->dev)
996                 return -EIO;
997
998         switch (cmd) {
999         case VIDIOCGCAP:
1000         {
1001                 struct video_capability *b = arg;
1002                 strcpy(b->name, se401->camera_name);
1003                 b->type = VID_TYPE_CAPTURE;
1004                 b->channels = 1;
1005                 b->audios = 0;
1006                 b->maxwidth = se401->width[se401->sizes-1];
1007                 b->maxheight = se401->height[se401->sizes-1];
1008                 b->minwidth = se401->width[0];
1009                 b->minheight = se401->height[0];
1010                 return 0;
1011         }
1012         case VIDIOCGCHAN:
1013         {
1014                 struct video_channel *v = arg;
1015
1016                 if (v->channel != 0)
1017                         return -EINVAL;
1018                 v->flags = 0;
1019                 v->tuners = 0;
1020                 v->type = VIDEO_TYPE_CAMERA;
1021                 strcpy(v->name, "Camera");
1022                 return 0;
1023         }
1024         case VIDIOCSCHAN:
1025         {
1026                 struct video_channel *v = arg;
1027
1028                 if (v->channel != 0)
1029                         return -EINVAL;
1030                 return 0;
1031         }
1032         case VIDIOCGPICT:
1033         {
1034                 struct video_picture *p = arg;
1035
1036                 se401_get_pict(se401, p);
1037                 return 0;
1038         }
1039         case VIDIOCSPICT:
1040         {
1041                 struct video_picture *p = arg;
1042
1043                 if (se401_set_pict(se401, p))
1044                         return -EINVAL;
1045                 return 0;
1046         }
1047         case VIDIOCSWIN:
1048         {
1049                 struct video_window *vw = arg;
1050
1051                 if (vw->flags)
1052                         return -EINVAL;
1053                 if (vw->clipcount)
1054                         return -EINVAL;
1055                 if (se401_set_size(se401, vw->width, vw->height))
1056                         return -EINVAL;
1057                 return 0;
1058         }
1059         case VIDIOCGWIN:
1060         {
1061                 struct video_window *vw = arg;
1062
1063                 vw->x = 0;               /* FIXME */
1064                 vw->y = 0;
1065                 vw->chromakey = 0;
1066                 vw->flags = 0;
1067                 vw->clipcount = 0;
1068                 vw->width = se401->cwidth;
1069                 vw->height = se401->cheight;
1070                 return 0;
1071         }
1072         case VIDIOCGMBUF:
1073         {
1074                 struct video_mbuf *vm = arg;
1075                 int i;
1076
1077                 memset(vm, 0, sizeof(*vm));
1078                 vm->size = SE401_NUMFRAMES * se401->maxframesize;
1079                 vm->frames = SE401_NUMFRAMES;
1080                 for (i = 0; i < SE401_NUMFRAMES; i++)
1081                         vm->offsets[i] = se401->maxframesize * i;
1082                 return 0;
1083         }
1084         case VIDIOCMCAPTURE:
1085         {
1086                 struct video_mmap *vm = arg;
1087
1088                 if (vm->format != VIDEO_PALETTE_RGB24)
1089                         return -EINVAL;
1090                 if (vm->frame >= SE401_NUMFRAMES)
1091                         return -EINVAL;
1092                 if (se401->frame[vm->frame].grabstate != FRAME_UNUSED)
1093                         return -EBUSY;
1094
1095                 /* Is this according to the v4l spec??? */
1096                 if (se401_set_size(se401, vm->width, vm->height))
1097                         return -EINVAL;
1098                 se401->frame[vm->frame].grabstate = FRAME_READY;
1099
1100                 if (!se401->streaming)
1101                         se401_start_stream(se401);
1102
1103                 /* Set the picture properties */
1104                 if (se401->framecount == 0)
1105                         se401_send_pict(se401);
1106                 /* Calibrate the reset level after a few frames. */
1107                 if (se401->framecount % 20 == 1)
1108                         se401_auto_resetlevel(se401);
1109
1110                 return 0;
1111         }
1112         case VIDIOCSYNC:
1113         {
1114                 int *frame = arg;
1115                 int ret = 0;
1116
1117                 if (*frame < 0 || *frame >= SE401_NUMFRAMES)
1118                         return -EINVAL;
1119
1120                 ret = se401_newframe(se401, *frame);
1121                 se401->frame[*frame].grabstate = FRAME_UNUSED;
1122                 return ret;
1123         }
1124         case VIDIOCGFBUF:
1125         {
1126                 struct video_buffer *vb = arg;
1127
1128                 memset(vb, 0, sizeof(*vb));
1129                 return 0;
1130         }
1131         case VIDIOCKEY:
1132                 return 0;
1133         case VIDIOCCAPTURE:
1134                 return -EINVAL;
1135         case VIDIOCSFBUF:
1136                 return -EINVAL;
1137         case VIDIOCGTUNER:
1138         case VIDIOCSTUNER:
1139                 return -EINVAL;
1140         case VIDIOCGFREQ:
1141         case VIDIOCSFREQ:
1142                 return -EINVAL;
1143         case VIDIOCGAUDIO:
1144         case VIDIOCSAUDIO:
1145                 return -EINVAL;
1146         default:
1147                 return -ENOIOCTLCMD;
1148         } /* end switch */
1149
1150         return 0;
1151 }
1152
1153 static long se401_ioctl(struct file *file,
1154                        unsigned int cmd, unsigned long arg)
1155 {
1156         return video_usercopy(file, cmd, arg, se401_do_ioctl);
1157 }
1158
1159 static ssize_t se401_read(struct file *file, char __user *buf,
1160                      size_t count, loff_t *ppos)
1161 {
1162         int realcount = count, ret = 0;
1163         struct video_device *dev = file->private_data;
1164         struct usb_se401 *se401 = (struct usb_se401 *)dev;
1165
1166
1167         if (se401->dev ==  NULL)
1168                 return -EIO;
1169         if (realcount > se401->cwidth*se401->cheight*3)
1170                 realcount = se401->cwidth*se401->cheight*3;
1171
1172         /* Shouldn't happen: */
1173         if (se401->frame[0].grabstate == FRAME_GRABBING)
1174                 return -EBUSY;
1175         se401->frame[0].grabstate = FRAME_READY;
1176         se401->frame[1].grabstate = FRAME_UNUSED;
1177         se401->curframe = 0;
1178
1179         if (!se401->streaming)
1180                 se401_start_stream(se401);
1181
1182         /* Set the picture properties */
1183         if (se401->framecount == 0)
1184                 se401_send_pict(se401);
1185         /* Calibrate the reset level after a few frames. */
1186         if (se401->framecount%20 == 1)
1187                 se401_auto_resetlevel(se401);
1188
1189         ret = se401_newframe(se401, 0);
1190
1191         se401->frame[0].grabstate = FRAME_UNUSED;
1192         if (ret)
1193                 return ret;
1194         if (copy_to_user(buf, se401->frame[0].data, realcount))
1195                 return -EFAULT;
1196
1197         return realcount;
1198 }
1199
1200 static int se401_mmap(struct file *file, struct vm_area_struct *vma)
1201 {
1202         struct video_device *dev = file->private_data;
1203         struct usb_se401 *se401 = (struct usb_se401 *)dev;
1204         unsigned long start = vma->vm_start;
1205         unsigned long size  = vma->vm_end-vma->vm_start;
1206         unsigned long page, pos;
1207
1208         mutex_lock(&se401->lock);
1209
1210         if (se401->dev ==  NULL) {
1211                 mutex_unlock(&se401->lock);
1212                 return -EIO;
1213         }
1214         if (size > (((SE401_NUMFRAMES * se401->maxframesize) + PAGE_SIZE - 1)
1215                                                         & ~(PAGE_SIZE - 1))) {
1216                 mutex_unlock(&se401->lock);
1217                 return -EINVAL;
1218         }
1219         pos = (unsigned long)se401->fbuf;
1220         while (size > 0) {
1221                 page = vmalloc_to_pfn((void *)pos);
1222                 if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED)) {
1223                         mutex_unlock(&se401->lock);
1224                         return -EAGAIN;
1225                 }
1226                 start +=  PAGE_SIZE;
1227                 pos +=  PAGE_SIZE;
1228                 if (size > PAGE_SIZE)
1229                         size -=  PAGE_SIZE;
1230                 else
1231                         size = 0;
1232         }
1233         mutex_unlock(&se401->lock);
1234
1235         return 0;
1236 }
1237
1238 static const struct v4l2_file_operations se401_fops = {
1239         .owner  =       THIS_MODULE,
1240         .open =         se401_open,
1241         .release =      se401_close,
1242         .read =         se401_read,
1243         .mmap =         se401_mmap,
1244         .ioctl =        se401_ioctl,
1245 };
1246 static struct video_device se401_template = {
1247         .name =         "se401 USB camera",
1248         .fops =         &se401_fops,
1249         .release = video_device_release_empty,
1250 };
1251
1252
1253
1254 /***************************/
1255 static int se401_init(struct usb_se401 *se401, int button)
1256 {
1257         int i = 0, rc;
1258         unsigned char cp[0x40];
1259         char temp[200];
1260         int slen;
1261
1262         /* led on */
1263         se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 1, NULL, 0);
1264
1265         /* get camera descriptor */
1266         rc = se401_sndctrl(0, se401, SE401_REQ_GET_CAMERA_DESCRIPTOR, 0,
1267                                                         cp, sizeof(cp));
1268         if (cp[1] != 0x41) {
1269                 err("Wrong descriptor type");
1270                 return 1;
1271         }
1272         slen = snprintf(temp, 200, "ExtraFeatures: %d", cp[3]);
1273
1274         se401->sizes = cp[4] + cp[5] * 256;
1275         se401->width = kmalloc(se401->sizes*sizeof(int), GFP_KERNEL);
1276         if (!se401->width)
1277                 return 1;
1278         se401->height = kmalloc(se401->sizes*sizeof(int), GFP_KERNEL);
1279         if (!se401->height) {
1280                 kfree(se401->width);
1281                 return 1;
1282         }
1283         for (i = 0; i < se401->sizes; i++) {
1284                 se401->width[i] = cp[6 + i * 4 + 0] + cp[6 + i*4 + 1] * 256;
1285                 se401->height[i] = cp[6 + i * 4 + 2] + cp[6 + i * 4 + 3] * 256;
1286         }
1287         slen += snprintf(temp + slen, 200 - slen, " Sizes:");
1288         for (i = 0; i < se401->sizes; i++) {
1289                 slen +=  snprintf(temp + slen, 200 - slen,
1290                         " %dx%d", se401->width[i], se401->height[i]);
1291         }
1292         dev_info(&se401->dev->dev, "%s\n", temp);
1293         se401->maxframesize = se401->width[se401->sizes-1] *
1294                                         se401->height[se401->sizes - 1] * 3;
1295
1296         rc = se401_sndctrl(0, se401, SE401_REQ_GET_WIDTH, 0, cp, sizeof(cp));
1297         se401->cwidth = cp[0]+cp[1]*256;
1298         rc = se401_sndctrl(0, se401, SE401_REQ_GET_HEIGHT, 0, cp, sizeof(cp));
1299         se401->cheight = cp[0]+cp[1]*256;
1300
1301         if (!(cp[2] & SE401_FORMAT_BAYER)) {
1302                 err("Bayer format not supported!");
1303                 return 1;
1304         }
1305         /* set output mode (BAYER) */
1306         se401_sndctrl(1, se401, SE401_REQ_SET_OUTPUT_MODE,
1307                                                 SE401_FORMAT_BAYER, NULL, 0);
1308
1309         rc = se401_sndctrl(0, se401, SE401_REQ_GET_BRT, 0, cp, sizeof(cp));
1310         se401->brightness = cp[0]+cp[1]*256;
1311         /* some default values */
1312         se401->resetlevel = 0x2d;
1313         se401->rgain = 0x20;
1314         se401->ggain = 0x20;
1315         se401->bgain = 0x20;
1316         se401_set_exposure(se401, 20000);
1317         se401->palette = VIDEO_PALETTE_RGB24;
1318         se401->enhance = 1;
1319         se401->dropped = 0;
1320         se401->error = 0;
1321         se401->framecount = 0;
1322         se401->readcount = 0;
1323
1324         /* Start interrupt transfers for snapshot button */
1325         if (button) {
1326                 se401->inturb = usb_alloc_urb(0, GFP_KERNEL);
1327                 if (!se401->inturb) {
1328                         dev_info(&se401->dev->dev,
1329                                  "Allocation of inturb failed\n");
1330                         return 1;
1331                 }
1332                 usb_fill_int_urb(se401->inturb, se401->dev,
1333                     usb_rcvintpipe(se401->dev, SE401_BUTTON_ENDPOINT),
1334                     &se401->button, sizeof(se401->button),
1335                     se401_button_irq,
1336                     se401,
1337                     8
1338                 );
1339                 if (usb_submit_urb(se401->inturb, GFP_KERNEL)) {
1340                         dev_info(&se401->dev->dev, "int urb burned down\n");
1341                         return 1;
1342                 }
1343         } else
1344                 se401->inturb = NULL;
1345
1346         /* Flash the led */
1347         se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 1, NULL, 0);
1348         se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 1, NULL, 0);
1349         se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 0, NULL, 0);
1350         se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 0, NULL, 0);
1351
1352         return 0;
1353 }
1354
1355 static int se401_probe(struct usb_interface *intf,
1356         const struct usb_device_id *id)
1357 {
1358         struct usb_device *dev = interface_to_usbdev(intf);
1359         struct usb_interface_descriptor *interface;
1360         struct usb_se401 *se401;
1361         char *camera_name = NULL;
1362         int button = 1;
1363
1364         /* We don't handle multi-config cameras */
1365         if (dev->descriptor.bNumConfigurations != 1)
1366                 return -ENODEV;
1367
1368         interface = &intf->cur_altsetting->desc;
1369
1370         /* Is it an se401? */
1371         if (le16_to_cpu(dev->descriptor.idVendor) ==  0x03e8 &&
1372             le16_to_cpu(dev->descriptor.idProduct) ==  0x0004) {
1373                 camera_name = "Endpoints/Aox SE401";
1374         } else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x0471 &&
1375             le16_to_cpu(dev->descriptor.idProduct) ==  0x030b) {
1376                 camera_name = "Philips PCVC665K";
1377         } else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x047d &&
1378             le16_to_cpu(dev->descriptor.idProduct) ==  0x5001) {
1379                 camera_name = "Kensington VideoCAM 67014";
1380         } else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x047d &&
1381             le16_to_cpu(dev->descriptor.idProduct) ==  0x5002) {
1382                 camera_name = "Kensington VideoCAM 6701(5/7)";
1383         } else if (le16_to_cpu(dev->descriptor.idVendor) ==  0x047d &&
1384             le16_to_cpu(dev->descriptor.idProduct) ==  0x5003) {
1385                 camera_name = "Kensington VideoCAM 67016";
1386                 button = 0;
1387         } else
1388                 return -ENODEV;
1389
1390         /* Checking vendor/product should be enough, but what the hell */
1391         if (interface->bInterfaceClass != 0x00)
1392                 return -ENODEV;
1393         if (interface->bInterfaceSubClass != 0x00)
1394                 return -ENODEV;
1395
1396         /* We found one */
1397         dev_info(&intf->dev, "SE401 camera found: %s\n", camera_name);
1398
1399         se401 = kzalloc(sizeof(*se401), GFP_KERNEL);
1400         if (se401 ==  NULL) {
1401                 err("couldn't kmalloc se401 struct");
1402                 return -ENOMEM;
1403         }
1404
1405         se401->dev = dev;
1406         se401->iface = interface->bInterfaceNumber;
1407         se401->camera_name = camera_name;
1408
1409         dev_info(&intf->dev, "firmware version: %02x\n",
1410                  le16_to_cpu(dev->descriptor.bcdDevice) & 255);
1411
1412         if (se401_init(se401, button)) {
1413                 kfree(se401);
1414                 return -EIO;
1415         }
1416
1417         memcpy(&se401->vdev, &se401_template, sizeof(se401_template));
1418         memcpy(se401->vdev.name, se401->camera_name,
1419                                         strlen(se401->camera_name));
1420         init_waitqueue_head(&se401->wq);
1421         mutex_init(&se401->lock);
1422         wmb();
1423
1424         if (video_register_device(&se401->vdev,
1425                                         VFL_TYPE_GRABBER, video_nr) < 0) {
1426                 kfree(se401);
1427                 err("video_register_device failed");
1428                 return -EIO;
1429         }
1430         dev_info(&intf->dev, "registered new video device: video%d\n",
1431                  se401->vdev.num);
1432
1433         usb_set_intfdata(intf, se401);
1434         return 0;
1435 }
1436
1437 static void se401_disconnect(struct usb_interface *intf)
1438 {
1439         struct usb_se401 *se401 = usb_get_intfdata(intf);
1440
1441         usb_set_intfdata(intf, NULL);
1442         if (se401) {
1443                 video_unregister_device(&se401->vdev);
1444                 if (!se401->user)
1445                         usb_se401_remove_disconnected(se401);
1446                 else {
1447                         se401->frame[0].grabstate = FRAME_ERROR;
1448                         se401->frame[0].grabstate = FRAME_ERROR;
1449
1450                         se401->streaming = 0;
1451
1452                         wake_up_interruptible(&se401->wq);
1453                         se401->removed = 1;
1454                 }
1455         }
1456 }
1457
1458 static struct usb_driver se401_driver = {
1459         .name            =  "se401",
1460         .id_table        =  device_table,
1461         .probe           =  se401_probe,
1462         .disconnect      =  se401_disconnect,
1463 };
1464
1465
1466
1467 /****************************************************************************
1468  *
1469  *  Module routines
1470  *
1471  ***************************************************************************/
1472
1473 static int __init usb_se401_init(void)
1474 {
1475         printk(KERN_INFO "SE401 usb camera driver version %s registering\n",
1476                                                                 version);
1477         if (flickerless)
1478                 if (flickerless != 50 && flickerless != 60) {
1479                         printk(KERN_ERR "Invallid flickerless value, use 0, 50 or 60.\n");
1480                         return -1;
1481         }
1482         return usb_register(&se401_driver);
1483 }
1484
1485 static void __exit usb_se401_exit(void)
1486 {
1487         usb_deregister(&se401_driver);
1488         printk(KERN_INFO "SE401 driver deregistered\frame");
1489 }
1490
1491 module_init(usb_se401_init);
1492 module_exit(usb_se401_exit);