usbvision-core.c

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

			*f++ = cb;
			*f++ = cg;
			*f++ = cr;
			scan_length += 3;
		}
	}

	frame->grabstate = FrameState_Done;
	frame->scanlength += scan_length;
	++num_pass;

}

/*
 * usbvision_decompress_alloc()
 *
 * allocates intermediate buffer for decompression
 */
int usbvision_decompress_alloc(struct usb_usbvision *usbvision)
{
	int IFB_size = MAX_FRAME_WIDTH * MAX_FRAME_HEIGHT * 3 / 2;
	usbvision->IntraFrameBuffer = vmalloc_32(IFB_size);
	if (usbvision->IntraFrameBuffer == NULL) {
		err("%s: unable to allocate %d for compr. frame buffer", __FUNCTION__, IFB_size);
		return -ENOMEM;
	}
	return 0;
}

/*
 * usbvision_decompress_free()
 *
 * frees intermediate buffer for decompression
 */
void usbvision_decompress_free(struct usb_usbvision *usbvision)
{
	if (usbvision->IntraFrameBuffer != NULL) {
		vfree(usbvision->IntraFrameBuffer);
		usbvision->IntraFrameBuffer = NULL;
	}
}

/************************************************************
 * Here comes the data parsing stuff that is run as interrupt
 ************************************************************/
/*
 * usbvision_find_header()
 *
 * Locate one of supported header markers in the scratch buffer.
 */
static enum ParseState usbvision_find_header(struct usb_usbvision *usbvision)
{
	struct usbvision_frame *frame;
	int foundHeader = 0;

	frame = usbvision->curFrame;

	while (scratch_get_header(usbvision, &frame->isocHeader) == USBVISION_HEADER_LENGTH) {
		// found header in scratch
		PDEBUG(DBG_HEADER, "found header: 0x%02x%02x %d %d %d %d %#x 0x%02x %u %u",
				frame->isocHeader.magic_2,
				frame->isocHeader.magic_1,
				frame->isocHeader.headerLength,
				frame->isocHeader.frameNum,
				frame->isocHeader.framePhase,
				frame->isocHeader.frameLatency,
				frame->isocHeader.dataFormat,
				frame->isocHeader.formatParam,
				frame->isocHeader.frameWidth,
				frame->isocHeader.frameHeight);

		if (usbvision->requestIntra) {
			if (frame->isocHeader.formatParam & 0x80) {
				foundHeader = 1;
				usbvision->lastIsocFrameNum = -1; // do not check for lost frames this time
				usbvision_unrequest_intra(usbvision);
				break;
			}
		}
		else {
			foundHeader = 1;
			break;
		}
	}

	if (foundHeader) {
		frame->frmwidth = frame->isocHeader.frameWidth * usbvision->stretch_width;
		frame->frmheight = frame->isocHeader.frameHeight * usbvision->stretch_height;
		frame->v4l2_linesize = (frame->frmwidth * frame->v4l2_format.depth)>> 3;
	}
	else { // no header found
		PDEBUG(DBG_HEADER, "skipping scratch data, no header");
		scratch_reset(usbvision);
		return ParseState_EndParse;
	}

	// found header
	if (frame->isocHeader.dataFormat==ISOC_MODE_COMPRESS) {
		//check isocHeader.frameNum for lost frames
		if (usbvision->lastIsocFrameNum >= 0) {
			if (((usbvision->lastIsocFrameNum + 1) % 32) != frame->isocHeader.frameNum) {
				// unexpected frame drop: need to request new intra frame
				PDEBUG(DBG_HEADER, "Lost frame before %d on USB", frame->isocHeader.frameNum);
				usbvision_request_intra(usbvision);
				return ParseState_NextFrame;
			}
		}
		usbvision->lastIsocFrameNum = frame->isocHeader.frameNum;
	}
	usbvision->header_count++;
	frame->scanstate = ScanState_Lines;
	frame->curline = 0;

	if (force_testpattern) {
		usbvision_testpattern(usbvision, 1, 1);
		return ParseState_NextFrame;
	}
	return ParseState_Continue;
}

static enum ParseState usbvision_parse_lines_422(struct usb_usbvision *usbvision,
					   long *pcopylen)
{
	volatile struct usbvision_frame *frame;
	unsigned char *f;
	int len;
	int i;
	unsigned char yuyv[4]={180, 128, 10, 128}; // YUV components
	unsigned char rv, gv, bv;	// RGB components
	int clipmask_index, bytes_per_pixel;
	int stretch_bytes, clipmask_add;

	frame  = usbvision->curFrame;
	f = frame->data + (frame->v4l2_linesize * frame->curline);

	/* Make sure there's enough data for the entire line */
	len = (frame->isocHeader.frameWidth * 2)+5;
	if (scratch_len(usbvision) < len) {
		PDEBUG(DBG_PARSE, "out of data in line %d, need %u.\n", frame->curline, len);
		return ParseState_Out;
	}

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

	bytes_per_pixel = frame->v4l2_format.bytes_per_pixel;
	stretch_bytes = (usbvision->stretch_width - 1) * bytes_per_pixel;
	clipmask_index = frame->curline * MAX_FRAME_WIDTH;
	clipmask_add = usbvision->stretch_width;

	for (i = 0; i < frame->frmwidth; i+=(2 * usbvision->stretch_width)) {

		scratch_get(usbvision, &yuyv[0], 4);

		if (frame->v4l2_format.format == V4L2_PIX_FMT_YUYV) {
			*f++ = yuyv[0]; // Y
			*f++ = yuyv[3]; // U
		}
		else {

			YUV_TO_RGB_BY_THE_BOOK(yuyv[0], yuyv[1], yuyv[3], rv, gv, bv);
			switch (frame->v4l2_format.format) {
				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;
				case V4L2_PIX_FMT_RGB555:
					*f++ = (0x1F & (bv >> 3)) | (0xE0 & (gv << 2));
					*f++ = (0x03 & (gv >> 6)) | (0x7C & (rv >> 1));
					break;
			}
		}
		clipmask_index += clipmask_add;
		f += stretch_bytes;

		if (frame->v4l2_format.format == V4L2_PIX_FMT_YUYV) {
			*f++ = yuyv[2]; // Y
			*f++ = yuyv[1]; // V
		}
		else {

			YUV_TO_RGB_BY_THE_BOOK(yuyv[2], yuyv[1], yuyv[3], rv, gv, bv);
			switch (frame->v4l2_format.format) {
				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;
				case V4L2_PIX_FMT_RGB555:
					*f++ = (0x1F & (bv >> 3)) | (0xE0 & (gv << 2));
					*f++ = (0x03 & (gv >> 6)) | (0x7C & (rv >> 1));
					break;
			}
		}
		clipmask_index += clipmask_add;
		f += stretch_bytes;
	}

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

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

/* The decompression routine  */
static int usbvision_decompress(struct usb_usbvision *usbvision,unsigned char *Compressed,
								unsigned char *Decompressed, int *StartPos,
								int *BlockTypeStartPos, int Len)
{
	int RestPixel, Idx, MaxPos, Pos, ExtraPos, BlockLen, BlockTypePos, BlockTypeLen;
	unsigned char BlockByte, BlockCode, BlockType, BlockTypeByte, Integrator;

	Integrator = 0;
	Pos = *StartPos;
	BlockTypePos = *BlockTypeStartPos;
	MaxPos = 396; //Pos + Len;
	ExtraPos = Pos;
	BlockLen = 0;
	BlockByte = 0;
	BlockCode = 0;
	BlockType = 0;
	BlockTypeByte = 0;
	BlockTypeLen = 0;
	RestPixel = Len;

	for (Idx = 0; Idx < Len; Idx++) {

		if (BlockLen == 0) {
			if (BlockTypeLen==0) {
				BlockTypeByte = Compressed[BlockTypePos];
				BlockTypePos++;
				BlockTypeLen = 4;
			}
			BlockType = (BlockTypeByte & 0xC0) >> 6;

			//statistic:
			usbvision->ComprBlockTypes[BlockType]++;

			Pos = ExtraPos;
			if (BlockType == 0) {
				if(RestPixel >= 24) {
					Idx += 23;
					RestPixel -= 24;
					Integrator = Decompressed[Idx];
				} else {
					Idx += RestPixel - 1;
					RestPixel = 0;
				}
			} else {
				BlockCode = Compressed[Pos];
				Pos++;
				if (RestPixel >= 24) {
					BlockLen  = 24;
				} else {
					BlockLen = RestPixel;
				}
				RestPixel -= BlockLen;
				ExtraPos = Pos + (BlockLen / 4);
			}
			BlockTypeByte <<= 2;
			BlockTypeLen -= 1;
		}
		if (BlockLen > 0) {
			if ((BlockLen%4) == 0) {
				BlockByte = Compressed[Pos];
				Pos++;
			}
			if (BlockType == 1) { //inter Block
				Integrator = Decompressed[Idx];
			}
			switch (BlockByte & 0xC0) {
				case 0x03<<6:
					Integrator += Compressed[ExtraPos];
					ExtraPos++;
					break;
				case 0x02<<6:
					Integrator += BlockCode;
					break;
				case 0x00:
					Integrator -= BlockCode;
					break;
			}
			Decompressed[Idx] = Integrator;
			BlockByte <<= 2;
			BlockLen -= 1;
		}
	}
	*StartPos = ExtraPos;
	*BlockTypeStartPos = BlockTypePos;
	return Idx;
}


/*
 * usbvision_parse_compress()
 *
 * Parse compressed frame 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_compress(struct usb_usbvision *usbvision,
					   long *pcopylen)
{
#define USBVISION_STRIP_MAGIC		0x5A
#define USBVISION_STRIP_LEN_MAX		400
#define USBVISION_STRIP_HEADER_LEN	3

	struct usbvision_frame *frame;
	unsigned char *f,*u = NULL ,*v = NULL;
	unsigned char StripData[USBVISION_STRIP_LEN_MAX];
	unsigned char StripHeader[USBVISION_STRIP_HEADER_LEN];
	int Idx, IdxEnd, StripLen, StripPtr, StartBlockPos, BlockPos, BlockTypePos;
	int clipmask_index, bytes_per_pixel, rc;
	int imageSize;
	unsigned char rv, gv, bv;
	static unsigned char *Y, *U, *V;

	frame  = usbvision->curFrame;
	imageSize = frame->frmwidth * frame->frmheight;
	if ( (frame->v4l2_format.format == V4L2_PIX_FMT_YUV422P) ||
	     (frame->v4l2_format.format == V4L2_PIX_FMT_YVU420) ) {       // this is a planar format
		//... v4l2_linesize not used here.
		f = frame->data + (frame->width * frame->curline);
	} else
		f = frame->data + (frame->v4l2_linesize * frame->curline);

	if (frame->v4l2_format.format == V4L2_PIX_FMT_YUYV){ //initialise u and v pointers
		// get base of u and b planes add halfoffset

		u = frame->data
			+ imageSize
			+ (frame->frmwidth >>1) * frame->curline ;
		v = u + (imageSize >>1 );

	} else if (frame->v4l2_format.format == V4L2_PIX_FMT_YVU420){

		v = frame->data + imageSize + ((frame->curline* (frame->width))>>2) ;
		u = v + (imageSize >>2) ;
	}

	if (frame->curline == 0) {
		usbvision_adjust_compression(usbvision);
	}

	if (scratch_len(usbvision) < USBVISION_STRIP_HEADER_LEN) {
		return ParseState_Out;
	}

	//get strip header without changing the scratch_read_ptr
	scratch_set_extra_ptr(usbvision, &StripPtr, 0);
	scratch_get_extra(usbvision, &StripHeader[0], &StripPtr,
				USBVISION_STRIP_HEADER_LEN);

	if (StripHeader[0] != USBVISION_STRIP_MAGIC) {
		// wrong strip magic
		usbvision->stripMagicErrors++;
		return ParseState_NextFrame;
	}

	if (frame->curline != (int)StripHeader[2]) {
		//line number missmatch error
		usbvision->stripLineNumberErrors++;
	}

	StripLen = 2 * (unsigned int)StripHeader[1];
	if (StripLen > USBVISION_STRIP_LEN_MAX) {
		// strip overrun
		// I think this never happens
		usbvision_request_intra(usbvision);
	}

	if (scratch_len(usbvision) < StripLen) {
		//there is not enough data for the strip
		return ParseState_Out;
	}

	if (usbvision->IntraFrameBuffer) {
		Y = usbvision->IntraFrameBuffer + frame->frmwidth * frame->curline;
		U = usbvision->IntraFrameBuffer + imageSize + (frame->frmwidth / 2) * (frame->curline / 2);
		V = usbvision->IntraFrameBuffer + imageSize / 4 * 5 + (frame->frmwidth / 2) * (frame->curline / 2);
	}
	else {
		return ParseState_NextFrame;
	}

	bytes_per_pixel = frame->v4l2_format.bytes_per_pixel;
	clipmask_index = frame->curline * MAX_FRAME_WIDTH;

	scratch_get(usbvision, StripData, StripLen);

	IdxEnd = frame->frmwidth;
	BlockTypePos = USBVISION_STRIP_HEADER_LEN;
	StartBlockPos = BlockTypePos + (IdxEnd - 1) / 96 + (IdxEnd / 2 - 1) / 96 + 2;
	BlockPos = StartBlockPos;

	usbvision->BlockPos = BlockPos;

	if ((rc = usbvision_decompress(usbvision, StripData, Y, &BlockPos, &BlockTypePos, IdxEnd)) != IdxEnd) {
		//return ParseState_Continue;
	}
	if (StripLen > usbvision->maxStripLen) {
		usbvision->maxStripLen = StripLen;
	}

	if (frame->curline%2) {
		if ((rc = usbvision_decompress(usbvision, StripData, V, &BlockPos, &BlockTypePos, IdxEnd/2)) != IdxEnd/2) {
		//return ParseState_Continue;
		}
	}
	else {
		if ((rc = usbvision_decompress(usbvision, StripData, U, &BlockPos, &BlockTypePos, IdxEnd/2)) != IdxEnd/2) {
			//return ParseState_Continue;
		}
	}

	if (BlockPos > usbvision->comprBlockPos) {
		usbvision->comprBlockPos = BlockPos;
	}
	if (BlockPos > StripLen) {
		usbvision->stripLenErrors++;
	}

	for (Idx = 0; Idx < IdxEnd; Idx++) {
		if(frame->v4l2_format.format == V4L2_PIX_FMT_YUYV) {
			*f++ = Y[Idx];
			*f++ = Idx & 0x01 ? U[Idx/2] : V[Idx/2];
		}
		else if(frame->v4l2_format.format == V4L2_PIX_FMT_YUV422P) {
			*f++ = Y[Idx];
			if ( Idx & 0x01)
				*u++ = U[Idx>>1] ;
			else
				*v++ = V[Idx>>1];