usbvision-core.c

来自「linux 内核源代码」· C语言 代码 · 共 2,307 行 · 第 1/5 页

		}
		else if (frame->v4l2_format.format == V4L2_PIX_FMT_YVU420) {
			*f++ = Y [Idx];
			if ( !((  Idx & 0x01  ) | (  frame->curline & 0x01  )) ){

/* 				 only need do this for 1 in 4 pixels */
/* 				 intraframe buffer is YUV420 format */

				*u++ = U[Idx >>1];
				*v++ = V[Idx >>1];
			}

		}
		else {
			YUV_TO_RGB_BY_THE_BOOK(Y[Idx], U[Idx/2], V[Idx/2], rv, gv, bv);
			switch (frame->v4l2_format.format) {
				case V4L2_PIX_FMT_GREY:
					*f++ = Y[Idx];
					break;
				case V4L2_PIX_FMT_RGB555:
					*f++ = (0x1F & (bv >> 3)) | (0xE0 & (gv << 2));
					*f++ = (0x03 & (gv >> 6)) | (0x7C & (rv >> 1));
					break;
				case V4L2_PIX_FMT_RGB565:
					*f++ = (0x1F & (bv >> 3)) | (0xE0 & (gv << 3));
					*f++ = (0x07 & (gv >> 5)) | (0xF8 &  rv);
					break;
				case V4L2_PIX_FMT_RGB24:
					*f++ = bv;
					*f++ = gv;
					*f++ = rv;
					break;
				case V4L2_PIX_FMT_RGB32:
					*f++ = bv;
					*f++ = gv;
					*f++ = rv;
					f++;
					break;
			}
		}
		clipmask_index++;
	}
	/* Deal with non-integer no. of bytes for YUV420P */
	if (frame->v4l2_format.format != V4L2_PIX_FMT_YVU420 )
		*pcopylen += frame->v4l2_linesize;
	else
		*pcopylen += frame->curline & 0x01 ? frame->v4l2_linesize : frame->v4l2_linesize << 1;

	frame->curline += 1;

	if (frame->curline >= frame->frmheight) {
		return ParseState_NextFrame;
	}
	else {
		return ParseState_Continue;
	}

}


/*
 * usbvision_parse_lines_420()
 *
 * Parse two lines from the scratch buffer, put
 * decoded RGB value into the current frame buffer and add the written
 * number of bytes (RGB) to the *pcopylen.
 *
 */
static enum ParseState usbvision_parse_lines_420(struct usb_usbvision *usbvision,
					   long *pcopylen)
{
	struct usbvision_frame *frame;
	unsigned char *f_even = NULL, *f_odd = NULL;
	unsigned int pixel_per_line, block;
	int pixel, block_split;
	int y_ptr, u_ptr, v_ptr, y_odd_offset;
	const int   y_block_size = 128;
	const int  uv_block_size = 64;
	const int sub_block_size = 32;
	const int y_step[] = { 0, 0, 0, 2 },  y_step_size = 4;
	const int uv_step[]= { 0, 0, 0, 4 }, uv_step_size = 4;
	unsigned char y[2], u, v;	/* YUV components */
	int y_, u_, v_, vb, uvg, ur;
	int r_, g_, b_;			/* RGB components */
	unsigned char g;
	int clipmask_even_index, clipmask_odd_index, bytes_per_pixel;
	int clipmask_add, stretch_bytes;

	frame  = usbvision->curFrame;
	f_even = frame->data + (frame->v4l2_linesize * frame->curline);
	f_odd  = f_even + frame->v4l2_linesize * usbvision->stretch_height;

	/* Make sure there's enough data for the entire line */
	/* In this mode usbvision transfer 3 bytes for every 2 pixels */
	/* I need two lines to decode the color */
	bytes_per_pixel = frame->v4l2_format.bytes_per_pixel;
	stretch_bytes = (usbvision->stretch_width - 1) * bytes_per_pixel;
	clipmask_even_index = frame->curline * MAX_FRAME_WIDTH;
	clipmask_odd_index  = clipmask_even_index + MAX_FRAME_WIDTH;
	clipmask_add = usbvision->stretch_width;
	pixel_per_line = frame->isocHeader.frameWidth;

	if (scratch_len(usbvision) < (int)pixel_per_line * 3) {
		//printk(KERN_DEBUG "out of data, need %d\n", len);
		return ParseState_Out;
	}

	if ((frame->curline + 1) >= frame->frmheight) {
		return ParseState_NextFrame;
	}

	block_split = (pixel_per_line%y_block_size) ? 1 : 0;	//are some blocks splitted into different lines?

	y_odd_offset = (pixel_per_line / y_block_size) * (y_block_size + uv_block_size)
			+ block_split * uv_block_size;

	scratch_set_extra_ptr(usbvision, &y_ptr, y_odd_offset);
	scratch_set_extra_ptr(usbvision, &u_ptr, y_block_size);
	scratch_set_extra_ptr(usbvision, &v_ptr, y_odd_offset
			+ (4 - block_split) * sub_block_size);

	for (block = 0; block < (pixel_per_line / sub_block_size);
	     block++) {


		for (pixel = 0; pixel < sub_block_size; pixel +=2) {
			scratch_get(usbvision, &y[0], 2);
			scratch_get_extra(usbvision, &u, &u_ptr, 1);
			scratch_get_extra(usbvision, &v, &v_ptr, 1);

			//I don't use the YUV_TO_RGB macro for better performance
			v_ = v - 128;
			u_ = u - 128;
			vb =              132252 * v_;
			uvg= -53281 * u_ - 25625 * v_;
			ur = 104595 * u_;

			if(frame->v4l2_format.format == V4L2_PIX_FMT_YUYV) {
				*f_even++ = y[0];
				*f_even++ = v;
			}
			else {
				y_ = 76284 * (y[0] - 16);

				b_ = (y_ + vb) >> 16;
				g_ = (y_ + uvg)>> 16;
				r_ = (y_ + ur) >> 16;

				switch (frame->v4l2_format.format) {
					case V4L2_PIX_FMT_RGB565:
						g = LIMIT_RGB(g_);
						*f_even++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 3));
						*f_even++ = (0x07 & (          g   >> 5)) | (0xF8 & LIMIT_RGB(r_));
						break;
					case V4L2_PIX_FMT_RGB24:
						*f_even++ = LIMIT_RGB(b_);
						*f_even++ = LIMIT_RGB(g_);
						*f_even++ = LIMIT_RGB(r_);
						break;
					case V4L2_PIX_FMT_RGB32:
						*f_even++ = LIMIT_RGB(b_);
						*f_even++ = LIMIT_RGB(g_);
						*f_even++ = LIMIT_RGB(r_);
						f_even++;
						break;
					case V4L2_PIX_FMT_RGB555:
						g = LIMIT_RGB(g_);
						*f_even++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 2));
						*f_even++ = (0x03 & (          g   >> 6)) |
							    (0x7C & (LIMIT_RGB(r_) >> 1));
						break;
				}
			}
			clipmask_even_index += clipmask_add;
			f_even += stretch_bytes;

			if(frame->v4l2_format.format == V4L2_PIX_FMT_YUYV) {
				*f_even++ = y[1];
				*f_even++ = u;
			}
			else {
				y_ = 76284 * (y[1] - 16);

				b_ = (y_ + vb) >> 16;
				g_ = (y_ + uvg)>> 16;
				r_ = (y_ + ur) >> 16;

				switch (frame->v4l2_format.format) {
					case V4L2_PIX_FMT_RGB565:
						g = LIMIT_RGB(g_);
						*f_even++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 3));
						*f_even++ = (0x07 & (          g   >> 5)) | (0xF8 & LIMIT_RGB(r_));
						break;
					case V4L2_PIX_FMT_RGB24:
						*f_even++ = LIMIT_RGB(b_);
						*f_even++ = LIMIT_RGB(g_);
						*f_even++ = LIMIT_RGB(r_);
						break;
					case V4L2_PIX_FMT_RGB32:
						*f_even++ = LIMIT_RGB(b_);
						*f_even++ = LIMIT_RGB(g_);
						*f_even++ = LIMIT_RGB(r_);
						f_even++;
						break;
					case V4L2_PIX_FMT_RGB555:
						g = LIMIT_RGB(g_);
						*f_even++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 2));
						*f_even++ = (0x03 & (          g   >> 6)) |
							    (0x7C & (LIMIT_RGB(r_) >> 1));
						break;
				}
			}
			clipmask_even_index += clipmask_add;
			f_even += stretch_bytes;

			scratch_get_extra(usbvision, &y[0], &y_ptr, 2);

			if(frame->v4l2_format.format == V4L2_PIX_FMT_YUYV) {
				*f_odd++ = y[0];
				*f_odd++ = v;
			}
			else {
				y_ = 76284 * (y[0] - 16);

				b_ = (y_ + vb) >> 16;
				g_ = (y_ + uvg)>> 16;
				r_ = (y_ + ur) >> 16;

				switch (frame->v4l2_format.format) {
					case V4L2_PIX_FMT_RGB565:
						g = LIMIT_RGB(g_);
						*f_odd++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 3));
						*f_odd++ = (0x07 & (          g   >> 5)) | (0xF8 & LIMIT_RGB(r_));
						break;
					case V4L2_PIX_FMT_RGB24:
						*f_odd++ = LIMIT_RGB(b_);
						*f_odd++ = LIMIT_RGB(g_);
						*f_odd++ = LIMIT_RGB(r_);
						break;
					case V4L2_PIX_FMT_RGB32:
						*f_odd++ = LIMIT_RGB(b_);
						*f_odd++ = LIMIT_RGB(g_);
						*f_odd++ = LIMIT_RGB(r_);
						f_odd++;
						break;
					case V4L2_PIX_FMT_RGB555:
						g = LIMIT_RGB(g_);
						*f_odd++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 2));
						*f_odd++ = (0x03 & (          g   >> 6)) |
							   (0x7C & (LIMIT_RGB(r_) >> 1));
						break;
				}
			}
			clipmask_odd_index += clipmask_add;
			f_odd += stretch_bytes;

			if(frame->v4l2_format.format == V4L2_PIX_FMT_YUYV) {
				*f_odd++ = y[1];
				*f_odd++ = u;
			}
			else {
				y_ = 76284 * (y[1] - 16);

				b_ = (y_ + vb) >> 16;
				g_ = (y_ + uvg)>> 16;
				r_ = (y_ + ur) >> 16;

				switch (frame->v4l2_format.format) {
					case V4L2_PIX_FMT_RGB565:
						g = LIMIT_RGB(g_);
						*f_odd++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 3));
						*f_odd++ = (0x07 & (          g   >> 5)) | (0xF8 & LIMIT_RGB(r_));
						break;
					case V4L2_PIX_FMT_RGB24:
						*f_odd++ = LIMIT_RGB(b_);
						*f_odd++ = LIMIT_RGB(g_);
						*f_odd++ = LIMIT_RGB(r_);
						break;
					case V4L2_PIX_FMT_RGB32:
						*f_odd++ = LIMIT_RGB(b_);
						*f_odd++ = LIMIT_RGB(g_);
						*f_odd++ = LIMIT_RGB(r_);
						f_odd++;
						break;
					case V4L2_PIX_FMT_RGB555:
						g = LIMIT_RGB(g_);
						*f_odd++ = (0x1F & (LIMIT_RGB(b_) >> 3)) | (0xE0 & (g << 2));
						*f_odd++ = (0x03 & (          g   >> 6)) |
							   (0x7C & (LIMIT_RGB(r_) >> 1));
						break;
				}
			}
			clipmask_odd_index += clipmask_add;
			f_odd += stretch_bytes;
		}

		scratch_rm_old(usbvision,y_step[block % y_step_size] * sub_block_size);
		scratch_inc_extra_ptr(&y_ptr, y_step[(block + 2 * block_split) % y_step_size]
				* sub_block_size);
		scratch_inc_extra_ptr(&u_ptr, uv_step[block % uv_step_size]
				* sub_block_size);
		scratch_inc_extra_ptr(&v_ptr, uv_step[(block + 2 * block_split) % uv_step_size]
				* sub_block_size);
	}

	scratch_rm_old(usbvision, pixel_per_line * 3 / 2
			+ block_split * sub_block_size);

	frame->curline += 2 * usbvision->stretch_height;
	*pcopylen += frame->v4l2_linesize * 2 * usbvision->stretch_height;

	if (frame->curline >= frame->frmheight)
		return ParseState_NextFrame;
	else
		return ParseState_Continue;
}

/*
 * usbvision_parse_data()
 *
 * Generic routine to parse the scratch buffer. It employs either
 * usbvision_find_header() or usbvision_parse_lines() to do most
 * of work.
 *
 */
static void usbvision_parse_data(struct usb_usbvision *usbvision)
{
	struct usbvision_frame *frame;
	enum ParseState newstate;
	long copylen = 0;
	unsigned long lock_flags;

	frame = usbvision->curFrame;

	PDEBUG(DBG_PARSE, "parsing len=%d\n", scratch_len(usbvision));

	while (1) {

		newstate = ParseState_Out;
		if (scratch_len(usbvision)) {
			if (frame->scanstate == ScanState_Scanning) {
				newstate = usbvision_find_header(usbvision);
			}
			else if (frame->scanstate == ScanState_Lines) {
				if (usbvision->isocMode == ISOC_MODE_YUV420) {
					newstate = usbvision_parse_lines_420(usbvision, &copylen);
				}
				else if (usbvision->isocMode == ISOC_MODE_YUV422) {
					newstate = usbvision_parse_lines_422(usbvision, &copylen);
				}
				else if (usbvision->isocMode == ISOC_MODE_COMPRESS) {
					newstate = usbvision_parse_compress(usbvision, &copylen);
				}

			}
		}
		if (newstate == ParseState_Continue) {
			continue;
		}
		else if ((newstate == ParseState_NextFrame) || (newstate == ParseState_Out)) {
			break;
		}
		else {
			return;	/* ParseState_EndParse */
		}
	}

	if (newstate == ParseState_NextFrame) {
		frame->grabstate = FrameState_Done;
		do_gettimeofday(&(frame->timestamp));
		frame->sequence = usbvision->frame_num;

		spin_lock_irqsave(&usbvision->queue_lock, lock_flags);
		list_move_tail(&(frame->frame), &usbvision->outqueue);
		usbvision->curFrame = NULL;
		spin_unlock_irqrestore(&usbvision->queue_lock, lock_flags);

		usbvision->frame_num++;

		/* This will cause the process to request another frame. */
		if (waitqueue_active(&usbvision->wait_frame)) {
			PDEBUG(DBG_PARSE, "Wake up !");
			wake_up_interruptible(&usbvision->wait_frame);
		}
	}
	else
		frame->grabstate = FrameState_Grabbing;


	/* Update the frame's uncompressed length. */
	frame->scanlength += copylen;
}


/*
 * Make all of the blocks of data contiguous
 */
static int usbvision_compress_isochronous(struct usb_usbvision *usbvision,
					  struct urb *urb)
{
	unsigned char *packet_data;
	int i, totlen = 0;

	for (i = 0; i < urb->number_of_packets; i++) {
		int packet_len = urb->iso_frame_desc[i].actual_length;
		int packet_stat = urb->iso_frame_desc[i].status;

		packet_data = urb->transfer_buffer + urb->iso_frame_desc[i].offset;

		/* Detect and ignore errored packets */
		if (packet_stat) {	// packet_stat != 0 ?????????????
			PDEBUG(DBG_ISOC, "data error: [%d] len=%d, status=%X", i, packet_len, packet_stat);
			usbvision->isocErrCount++;
			continue;
		}

		/* Detect and ignore empty packets */
		if (packet_len < 0) {
			PDEBUG(DBG_ISOC, "error packet [%d]", i);
			usbvision->isocSkipCount++;
			continue;
		}
		else if (packet_len == 0) {	/* Frame end ????? */
			PDEBUG(DBG_ISOC, "null packet [%d]", i);
			usbvision->isocstate=IsocState_NoFrame;
			usbvision->isocSkipCount++;
			continue;
		}
		else if (packet_len > usbvision->isocPacketSize) {
			PDEBUG(DBG_ISOC, "packet[%d] > isocPacketSize", i);
			usbvision->isocSkipCount++;
			continue;
		}

		PDEBUG(DBG_ISOC, "packet ok [%d] len=%d", i, packet_len);

		if (usbvision->isocstate==IsocState_NoFrame) { //new frame begins
			usbvision->isocstate=IsocState_InFrame;
			scratch_mark_header(usbvision);
			usbvision_measure_bandwidth(usbvision);
			PDEBUG(DBG_ISOC, "packet with header");
		}

		/*
		 * If usbvision continues to feed us with data but there is no
		 * consumption (if, for example, V4L client fell asleep) we
		 * may overflow the buffer. We have to move old data over to
		 * free room for new data. This is bad for old data. If we
		 * just drop new data then it's bad for new data... choose
		 * your favorite evil here.
		 */
		if (scratch_free(usbvision) < packet_len) {

			usbvision->scratch_ovf_count++;
			PDEBUG(DBG_ISOC, "scratch buf overflow! scr_len: %d, n: %d",
			       scratch_len(usbvision), packet_len);
			scratch_rm_old(usbvision, packet_len - scratch_free(usbvision));
		}

		/* Now we know that there is enough room in scratch buffer */
		scratch_put(usbvision, packet_data, packet_len);
		totlen += packet_len;