se401.c

基于S3CEB2410平台LINUX操作系统下 USB驱动源代码

	if(usb_submit_urb(urb))
		info("urb burned down");
	return;
}

static void se401_send_size(struct usb_se401 *se401, int width, int height)
{
	int i=0;
	int mode=0x03; /* No compression */
	int sendheight=height;
	int sendwidth=width;

	/* JangGu compression can only be used with the camera supported sizes,
	   but bayer seems to work with any size that fits on the sensor.
	   We check if we can use compression with the current size with either
	   4 or 16 times subcapturing, if not we use uncompressed bayer data
	   but this will result in cutouts of the maximum size....
	 */
	while (i<se401->sizes && !(se401->width[i]==width && se401->height[i]==height))
		i++;
	while (i<se401->sizes) {
		if (se401->width[i]==width*2 && se401->height[i]==height*2) {
			sendheight=se401->height[i];
			sendwidth=se401->width[i];
			mode=0x40;
		}
		if (se401->width[i]==width*4 && se401->height[i]==height*4) {
			sendheight=se401->height[i];
			sendwidth=se401->width[i];
			mode=0x42;
		}
		i++;
	}

	se401_sndctrl(1, se401, SE401_REQ_SET_WIDTH, sendwidth, NULL, 0);
	se401_sndctrl(1, se401, SE401_REQ_SET_HEIGHT, sendheight, NULL, 0);
	se401_set_feature(se401, SE401_OPERATINGMODE, mode);

	if (mode==0x03) {
		se401->format=FMT_BAYER;
	} else {
		se401->format=FMT_JANGGU;
	}

	return;
}

/*
	In this function se401_send_pict is called several times,
	for some reason (depending on the state of the sensor and the phase of
	the moon :) doing this only in either place doesn't always work...
*/
static int se401_start_stream(struct usb_se401 *se401)
{
	urb_t *urb;
	int err=0, i;
	se401->streaming=1;

        se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 1, NULL, 0);
        se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 1, NULL, 0);

	/* Set picture settings */
	se401_set_feature(se401, HV7131_REG_MODE_B, 0x05);/*windowed + pix intg */
	se401_send_pict(se401);

	se401_send_size(se401, se401->cwidth, se401->cheight);

	se401_sndctrl(1, se401, SE401_REQ_START_CONTINUOUS_CAPTURE, 0, NULL, 0);

	/* Do some memory allocation */
	for (i=0; i<SE401_NUMFRAMES; i++) {
		se401->frame[i].data=se401->fbuf + i * se401->maxframesize;
		se401->frame[i].curpix=0;
	}
	for (i=0; i<SE401_NUMSBUF; i++) {
		se401->sbuf[i].data=kmalloc(SE401_PACKETSIZE, GFP_KERNEL);
	}

	se401->bayeroffset=0;
	se401->scratch_next=0;
	se401->scratch_use=0;
	se401->scratch_overflow=0;
	for (i=0; i<SE401_NUMSCRATCH; i++) {
		se401->scratch[i].data=kmalloc(SE401_PACKETSIZE, GFP_KERNEL);
		se401->scratch[i].state=BUFFER_UNUSED;
	}

	for (i=0; i<SE401_NUMSBUF; i++) {
		urb=usb_alloc_urb(0);
		if(!urb)
			return ENOMEM;

		FILL_BULK_URB(urb, se401->dev,
			usb_rcvbulkpipe(se401->dev, SE401_VIDEO_ENDPOINT),
			se401->sbuf[i].data, SE401_PACKETSIZE,
			se401_video_irq,
			se401);
		urb->transfer_flags |= USB_QUEUE_BULK;

		se401->urb[i]=urb;

		err=usb_submit_urb(se401->urb[i]);
		if(err)
			err("urb burned down");
	}

	se401->framecount=0;

	return 0;
}

static int se401_stop_stream(struct usb_se401 *se401)
{
	int i;

	if (!se401->streaming || !se401->dev)
		return 1;

	se401->streaming=0;

	se401_sndctrl(1, se401, SE401_REQ_STOP_CONTINUOUS_CAPTURE, 0, NULL, 0);

	se401_sndctrl(1, se401, SE401_REQ_LED_CONTROL, 0, NULL, 0);
	se401_sndctrl(1, se401, SE401_REQ_CAMERA_POWER, 0, NULL, 0);

	for (i=0; i<SE401_NUMSBUF; i++) if (se401->urb[i]) {
		se401->urb[i]->next=NULL;
		usb_unlink_urb(se401->urb[i]);
		usb_free_urb(se401->urb[i]);
		se401->urb[i]=NULL;
		kfree(se401->sbuf[i].data);
	}
	for (i=0; i<SE401_NUMSCRATCH; i++) {
		kfree(se401->scratch[i].data);
		se401->scratch[i].data=NULL;
	}

	return 0;
}

static int se401_set_size(struct usb_se401 *se401, int width, int height)
{
	int wasstreaming=se401->streaming;
	/* Check to see if we need to change */
	if (se401->cwidth==width && se401->cheight==height)
		return 0;

	/* Check for a valid mode */
	if (!width || !height)
		return 1;
	if ((width & 1) || (height & 1))
		return 1;
	if (width>se401->width[se401->sizes-1])
		return 1;
	if (height>se401->height[se401->sizes-1])
		return 1;

	/* Stop a current stream and start it again at the new size */
	if (wasstreaming)
		se401_stop_stream(se401);
	se401->cwidth=width;
	se401->cheight=height;
	if (wasstreaming)
		se401_start_stream(se401);
	return 0;
}


/****************************************************************************
 *
 * Video Decoding
 *
 ***************************************************************************/

/*
	This shouldn't really be done in a v4l driver....
	But it does make the image look a lot more usable.
	Basicly it lifts the dark pixels more than the light pixels.
*/
static inline void enhance_picture(unsigned char *frame, int len)
{
	while (len--) {
		*frame++=(((*frame^255)*(*frame^255))/255)^255;
	}
}

static inline void decode_JangGu_integrate(struct usb_se401 *se401, int data)
{
	struct se401_frame *frame=&se401->frame[se401->curframe];
	int linelength=se401->cwidth*3;

	if (frame->curlinepix >= linelength) {
		frame->curlinepix=0;
		frame->curline+=linelength;
	}

	/* First three are absolute, all others relative.
	 * Format is rgb from right to left (mirrorred image), 
	 * we flip it to get bgr from left to right. */
	if (frame->curlinepix < 3) {
		*(frame->curline-frame->curlinepix)=1+data*4;
	} else {
		*(frame->curline-frame->curlinepix)=
		    *(frame->curline-frame->curlinepix+3)+data*4;
	}
	frame->curlinepix++;
}

static inline void decode_JangGu_vlc (struct usb_se401 *se401, unsigned char *data, int bit_exp, int packetlength)
{
	int pos=0;
	int vlc_cod=0;
	int vlc_size=0;
	int vlc_data=0;
	int bit_cur;
	int bit;
	data+=4;
	while (pos < packetlength) {
		bit_cur=8;
		while (bit_cur && bit_exp) {
			bit=((*data)>>(bit_cur-1))&1;
			if (!vlc_cod) {
				if (bit) {
					vlc_size++;
				} else {
					if (!vlc_size) {
						decode_JangGu_integrate(se401, 0);
					} else {
						vlc_cod=2;
						vlc_data=0;
					}
				}
			} else {
				if (vlc_cod==2) {
					if (!bit) vlc_data=-(1<<vlc_size)+1;
					vlc_cod--;
				}
				vlc_size--;
				vlc_data+=bit<<vlc_size;
				if (!vlc_size) {
					decode_JangGu_integrate(se401, vlc_data);
					vlc_cod=0;
				}
			}
			bit_cur--;
			bit_exp--;
		}
		pos++;
		data++;
	}
}

static inline void decode_JangGu (struct usb_se401 *se401, struct se401_scratch *buffer)
{
	unsigned char *data=buffer->data;
	int len=buffer->length;
	int bit_exp=0, pix_exp=0, frameinfo=0, packetlength=0, size;
	int datapos=0;

	/* New image? */
	if (!se401->frame[se401->curframe].curpix) {
		se401->frame[se401->curframe].curlinepix=0;
		se401->frame[se401->curframe].curline=
		    se401->frame[se401->curframe].data+
		    se401->cwidth*3-1;
		if (se401->frame[se401->curframe].grabstate==FRAME_READY)
			se401->frame[se401->curframe].grabstate=FRAME_GRABBING;
		se401->vlcdatapos=0;
	}
	while (datapos < len) {
		size=1024-se401->vlcdatapos;
		if (size+datapos > len)
			size=len-datapos;
		memcpy(se401->vlcdata+se401->vlcdatapos, data+datapos, size);
		se401->vlcdatapos+=size;
		packetlength=0;
		if (se401->vlcdatapos >= 4) {
			bit_exp=se401->vlcdata[3]+(se401->vlcdata[2]<<8);
			pix_exp=se401->vlcdata[1]+((se401->vlcdata[0]&0x3f)<<8);
			frameinfo=se401->vlcdata[0]&0xc0;
			packetlength=((bit_exp+47)>>4)<<1;
			if (packetlength > 1024) {
				se401->vlcdatapos=0;
				datapos=len;
				packetlength=0;
				se401->error++;
				se401->frame[se401->curframe].curpix=0;
			}
		}
		if (packetlength && se401->vlcdatapos >= packetlength) {
			decode_JangGu_vlc(se401, se401->vlcdata, bit_exp, packetlength);
			se401->frame[se401->curframe].curpix+=pix_exp*3;
			datapos+=size-(se401->vlcdatapos-packetlength);
			se401->vlcdatapos=0;
			if (se401->frame[se401->curframe].curpix>=se401->cwidth*se401->cheight*3) {
				if (se401->frame[se401->curframe].curpix==se401->cwidth*se401->cheight*3) {
					if (se401->frame[se401->curframe].grabstate==FRAME_GRABBING) {
						se401->frame[se401->curframe].grabstate=FRAME_DONE;
						se401->framecount++;
						se401->readcount++;
					}
					if (se401->frame[(se401->curframe+1)&(SE401_NUMFRAMES-1)].grabstate==FRAME_READY) {
						se401->curframe=(se401->curframe+1) & (SE401_NUMFRAMES-1);
					}
				} else {
					se401->error++;
				}
				se401->frame[se401->curframe].curpix=0;
				datapos=len;
			}
		} else {
			datapos+=size;
		}
	}
}

static inline void decode_bayer (struct usb_se401 *se401, struct se401_scratch *buffer)
{
	unsigned char *data=buffer->data;
	int len=buffer->length;
	int offset=buffer->offset;
	int datasize=se401->cwidth*se401->cheight;
	struct se401_frame *frame=&se401->frame[se401->curframe];

	unsigned char *framedata=frame->data, *curline, *nextline;
	int width=se401->cwidth;
	int blineoffset=0, bline;
	int linelength=width*3, i;
	

	if (frame->curpix==0) {
		if (frame->grabstate==FRAME_READY) {
			frame->grabstate=FRAME_GRABBING;
		}
		frame->curline=framedata+linelength;
		frame->curlinepix=0;
	}

	if (offset!=frame->curpix) {
		/* Regard frame as lost :( */
		frame->curpix=0;
		se401->error++;
		return;
	}

	/* Check if we have to much data */
	if (frame->curpix+len > datasize) {
		len=datasize-frame->curpix;
	}
	if (se401->cheight%4)
		blineoffset=1;
	bline=frame->curpix/se401->cwidth+blineoffset;

	curline=frame->curline;
	nextline=curline+linelength;
	if (nextline >= framedata+datasize*3)
		nextline=curline;
	while (len) {
		if (frame->curlinepix>=width) {
			frame->curlinepix-=width;
			bline=frame->curpix/width+blineoffset;
			curline+=linelength*2;
			nextline+=linelength*2;
			if (curline >= framedata+datasize*3) {
				frame->curlinepix++;
				curline-=3;
				nextline-=3;
				len--;
				data++;
				frame->curpix++;
			}
			if (nextline >= framedata+datasize*3)
				nextline=curline;
		}
		if ((bline&1)) {
			if ((frame->curlinepix&1)) {
				*(curline+2)=*data;
				*(curline-1)=*data;
				*(nextline+2)=*data;
				*(nextline-1)=*data;
			} else {
				*(curline+1)=
					(*(curline+1)+*data)/2;
				*(curline-2)=
					(*(curline-2)+*data)/2;
				*(nextline+1)=*data;
				*(nextline-2)=*data;
			}
		} else {
			if ((frame->curlinepix&1)) {
				*(curline+1)=
					(*(curline+1)+*data)/2;
				*(curline-2)=
					(*(curline-2)+*data)/2;
				*(nextline+1)=*data;
				*(nextline-2)=*data;
			} else {
				*curline=*data;
				*(curline-3)=*data;
				*nextline=*data;
				*(nextline-3)=*data;
			}
		}
		frame->curlinepix++;
		curline-=3;
		nextline-=3;
		len--;
		data++;
		frame->curpix++;
	}
	frame->curline=curline;

	if (frame->curpix>=datasize) {
		/* Fix the top line */
		framedata+=linelength;
		for (i=0; i<linelength; i++) {
			*--framedata=*(framedata+linelength);
		}
		/* Fix the left side (green is already present) */
		for (i=0; i<se401->cheight; i++) {
			*framedata=*(framedata+3);
			*(framedata+1)=*(framedata+4);
			*(framedata+2)=*(framedata+5);
			framedata+=linelength;
		}
		frame->curpix=0;
		frame->grabstate=FRAME_DONE;
		se401->framecount++;
		se401->readcount++;
		if (se401->frame[(se401->curframe+1)&(SE401_NUMFRAMES-1)].grabstate==FRAME_READY) {
			se401->curframe=(se401->curframe+1) & (SE401_NUMFRAMES-1);
		}
	}
}

static int se401_newframe(struct usb_se401 *se401, int framenr)
{
	DECLARE_WAITQUEUE(wait, current);
	int errors=0;

	while (se401->streaming &&
	    (se401->frame[framenr].grabstate==FRAME_READY ||
	     se401->frame[framenr].grabstate==FRAME_GRABBING) ) {
		if(!se401->frame[framenr].curpix) {
			errors++;
		}
		wait_interruptible(
		    se401->scratch[se401->scratch_use].state!=BUFFER_READY,
		    &se401->wq,
		    &wait
		);
		if (se401->nullpackets > SE401_MAX_NULLPACKETS) {
			se401->nullpackets=0;
			info("to many null length packets, restarting capture");
			se401_stop_stream(se401);
			se401_start_stream(se401);			
		} else {
			if (se401->scratch[se401->scratch_use].state!=BUFFER_READY) {
				se401->frame[framenr].grabstate=FRAME_ERROR;
				return -EIO;
			}
			se401->scratch[se401->scratch_use].state=BUFFER_BUSY;
			if (se401->format==FMT_JANGGU) {
				decode_JangGu(se401, &se401->scratch[se401->scratch_use]);
			} else {
				decode_bayer(se401, &se401->scratch[se401->scratch_use]);
			}
			se401->scratch[se401->scratch_use].state=BUFFER_UNUSED;
			se401->scratch_use++;
			if (se401->scratch_use>=SE401_NUMSCRATCH)
				se401->scratch_use=0;
			if (errors > SE401_MAX_ERRORS) {
				errors=0;
				info("to much errors, restarting capture");
				se401_stop_stream(se401);
				se401_start_stream(se401);
			}
		}
	}

	if (se401->frame[framenr].grabstate==FRAME_DONE)
		if (se401->enhance)
			enhance_picture(se401->frame[framenr].data, se401->cheight*se401->cwidth*3);
	return 0;
}


/****************************************************************************
 *
 * Video4Linux
 *
 ***************************************************************************/


static int se401_open(struct video_device *dev, int flags)
{
	struct usb_se401 *se401 = (struct usb_se401 *)dev;
	int err = 0;

	/* we are called with the BKL held */
	MOD_INC_USE_COUNT;

	se401->user=1;
	se401->fbuf=rvmalloc(se401->maxframesize * SE401_NUMFRAMES);
	if(!se401->fbuf) err=-ENOMEM;

        if (err) {
		MOD_DEC_USE_COUNT;
		se401->user = 0;
	}

	return err;
}

static void se401_close(struct video_device *dev)
{
	/* called with BKL held */
        struct usb_se401 *se401 = (struct usb_se401 *)dev;
	int i;

	for (i=0; i<SE401_NUMFRAMES; i++)
		se401->frame[i].grabstate=FRAME_UNUSED;
	if (se401->streaming)
		se401_stop_stream(se401);

	rvfree(se401->fbuf, se401->maxframesize * SE401_NUMFRAMES);
	se401->user=0;

        if (se401->removed) {
                video_unregister_device(&se401->vdev);
		kfree(se401->width);
		kfree(se401->height);
                kfree(se401);
                se401 = NULL;
		info("device unregistered");
	}

        MOD_DEC_USE_COUNT;
}

static int se401_init_done(struct video_device *dev)
{
#if defined(CONFIG_PROC_FS) && defined(CONFIG_VIDEO_PROC_FS)
        create_proc_se401_cam((struct usb_se401 *)dev);
#endif

        return 0;
}

static long se401_write(struct video_device *dev, const char *buf, unsigned long
 count, int noblock)
{
        return -EINVAL;
}

static int se401_ioctl(struct video_device *vdev, unsigned int cmd, void *arg)
{
        struct usb_se401 *se401 = (struct usb_se401 *)vdev;