se401.c

linux-2.6.15.6

	}

	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]) {
		usb_kill_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.
	Basically 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;
		frame++;
	}
}

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=*(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;
}

static void usb_se401_remove_disconnected (struct usb_se401 *se401)
{
	int i;

        se401->dev = NULL;

	for (i=0; i<SE401_NUMSBUF; i++)
		if (se401->urb[i]) {
			usb_kill_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);
	}
	if (se401->inturb) {
		usb_kill_urb(se401->inturb);
		usb_free_urb(se401->inturb);
	}
        info("%s disconnected", se401->camera_name);

        /* Free the memory */
	kfree(se401->width);
	kfree(se401->height);
	kfree(se401);
}



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


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

	if (se401->user)
		return -EBUSY;
	se401->fbuf = rvmalloc(se401->maxframesize * SE401_NUMFRAMES);
	if (se401->fbuf)
		file->private_data = dev;
	else 
		err = -ENOMEM;
	se401->user = !err;

	return err;
}

static int se401_close(struct inode *inode, struct file *file)
{
	struct video_device *dev = file->private_data;
        struct usb_se401 *se401 = (struct usb_se401 *)dev;
	int i;

	rvfree(se401->fbuf, se401->maxframesize * SE401_NUMFRAMES);
        if (se401->removed) {
		usb_se401_remove_disconnected(se401);
		info("device unregistered");
	} else {
		for (i=0; i<SE401_NUMFRAMES; i++)
			se401->frame[i].grabstate=FRAME_UNUSED;
		if (se401->streaming)
			se401_stop_stream(se401);
		se401->user=0;
	}
	file->private_data = NULL;
	return 0;
}

static int se401_do_ioctl(struct inode *inode, struct file *file,
			  unsigned int cmd, void *arg)
{
	struct video_device *vdev = file->private_data;
        struct usb_se401 *se401 = (struct usb_se401 *)vdev;

        if (!se401->dev)
                return -EIO;

        switch (cmd) {
	case VIDIOCGCAP:
	{
		struct video_capability *b = arg;
		strcpy(b->name, se401->camera_name);
		b->type = VID_TYPE_CAPTURE;
		b->channels = 1;
		b->audios = 0;
		b->maxwidth = se401->width[se401->sizes-1];
		b->maxheight = se401->height[se401->sizes-1];
		b->minwidth = se401->width[0];