qc-mjpeg.c

在Linux下用于webeye的摄像头的驱动

	{  -151, 16,   1 },
	{   152, 16,   1 },
	{  -152, 16,   1 },
	{   153, 16,   1 },
	{  -153, 16,   1 },
	{   154, 16,   1 },
	{  -154, 16,   1 },
	{   155, 16,   1 },
	{  -155, 16,   1 },
	{   156, 16,   1 },
	{  -156, 16,   1 },
	{   157, 16,   1 },
	{  -157, 16,   1 },
	{   158, 16,   1 },
	{  -158, 16,   1 },
	{   159, 16,   1 },
	{  -159, 16,   1 },
	{   160, 16,   1 },
	{  -160, 16,   1 },
	{   161, 16,   1 },
	{  -161, 16,   1 },
	{   162, 16,   1 },
	{  -162, 16,   1 },
	{   163, 16,   1 },
	{  -163, 16,   1 },
	{   164, 16,   1 },
	{  -164, 16,   1 },
	{   165, 16,   1 },
	{  -165, 16,   1 },
	{   166, 16,   1 },
	{  -166, 16,   1 },
	{   167, 16,   1 },
	{  -167, 16,   1 },
	{   168, 16,   1 },
	{  -168, 16,   1 },
	{   169, 16,   1 },
	{  -169, 16,   1 },
	{   170, 16,   1 },
	{  -170, 16,   1 },
	{   171, 16,   1 },
	{  -171, 16,   1 },
	{   172, 16,   1 },
	{  -172, 16,   1 },
	{   173, 16,   1 },
	{  -173, 16,   1 },
	{   174, 16,   1 },
	{  -174, 16,   1 },
	{   175, 16,   1 },
	{  -175, 16,   1 },
	{   176, 16,   1 },
	{  -176, 16,   1 },
	{   177, 16,   1 },
	{  -177, 16,   1 },
	{   178, 16,   1 },
	{  -178, 16,   1 },
	{   179, 16,   1 },
	{  -179, 16,   1 },
	{   180, 16,   1 },
	{  -180, 16,   1 },
	{   181, 16,   1 },
	{  -181, 16,   1 },
	{   182, 16,   1 },
	{  -182, 16,   1 },
	{   183, 16,   1 },
	{  -183, 16,   1 },
	{   184, 16,   1 },
	{  -184, 16,   1 },
	{   185, 16,   1 },
	{  -185, 16,   1 },
	{   186, 16,   1 },
	{  -186, 16,   1 },
	{   187, 16,   1 },
	{  -187, 16,   1 },
	{   188, 16,   1 },
	{  -188, 16,   1 },
	{   189, 16,   1 },
	{  -189, 16,   1 },
	{   190, 16,   1 },
	{  -190, 16,   1 },
	{   191, 16,   1 },
	{  -191, 16,   1 },
	{   192, 16,   1 },
	{  -192, 16,   1 },
	{   193, 16,   1 },
	{  -193, 16,   1 },
	{   194, 16,   1 },
	{  -194, 16,   1 },
	{   195, 16,   1 },
	{  -195, 16,   1 },
	{   196, 16,   1 },
	{  -196, 16,   1 },
	{   197, 16,   1 },
	{  -197, 16,   1 },
	{   198, 16,   1 },
	{  -198, 16,   1 },
	{   199, 16,   1 },
	{  -199, 16,   1 },
	{   200, 16,   1 },
	{  -200, 16,   1 },
	{   201, 16,   1 },
	{  -201, 16,   1 },
	{   202, 16,   1 },
	{  -202, 16,   1 },
	{   203, 16,   1 },
	{  -203, 16,   1 },
	{   204, 16,   1 },
	{  -204, 16,   1 },
	{   205, 16,   1 },
	{  -205, 16,   1 },
	{   206, 16,   1 },
	{  -206, 16,   1 },
	{   207, 16,   1 },
	{  -207, 16,   1 },
	{   208, 16,   1 },
	{  -208, 16,   1 },
	{   209, 16,   1 },
	{  -209, 16,   1 },
	{   210, 16,   1 },
	{  -210, 16,   1 },
	{   211, 16,   1 },
	{  -211, 16,   1 },
	{   212, 16,   1 },
	{  -212, 16,   1 },
	{   213, 16,   1 },
	{  -213, 16,   1 },
	{   214, 16,   1 },
	{  -214, 16,   1 },
	{   215, 16,   1 },
	{  -215, 16,   1 },
	{   216, 16,   1 },
	{  -216, 16,   1 },
	{   217, 16,   1 },
	{  -217, 16,   1 },
	{   218, 16,   1 },
	{  -218, 16,   1 },
	{   219, 16,   1 },
	{  -219, 16,   1 },
	{   220, 16,   1 },
	{  -220, 16,   1 },
	{   221, 16,   1 },
	{  -221, 16,   1 },
	{   222, 16,   1 },
	{  -222, 16,   1 },
	{   223, 16,   1 },
	{  -223, 16,   1 },
	{   224, 16,   1 },
	{  -224, 16,   1 },
	{   225, 16,   1 },
	{  -225, 16,   1 },
	{   226, 16,   1 },
	{  -226, 16,   1 },
	{   227, 16,   1 },
	{  -227, 16,   1 },
	{   228, 16,   1 },
	{  -228, 16,   1 },
	{   229, 16,   1 },
	{  -229, 16,   1 },
	{   230, 16,   1 },
	{  -230, 16,   1 },
	{   231, 16,   1 },
	{  -231, 16,   1 },
	{   232, 16,   1 },
	{  -232, 16,   1 },
	{   233, 16,   1 },
	{  -233, 16,   1 },
	{   234, 16,   1 },
	{  -234, 16,   1 },
	{   235, 16,   1 },
	{  -235, 16,   1 },
	{   236, 16,   1 },
	{  -236, 16,   1 },
	{   237, 16,   1 },
	{  -237, 16,   1 },
	{   238, 16,   1 },
	{  -238, 16,   1 },
	{   239, 16,   1 },
	{  -239, 16,   1 },
	{   240, 16,   1 },
	{  -240, 16,   1 },
	{   241, 16,   1 },
	{  -241, 16,   1 },
	{   242, 16,   1 },
	{  -242, 16,   1 },
	{   243, 16,   1 },
	{  -243, 16,   1 },
	{   244, 16,   1 },
	{  -244, 16,   1 },
	{   245, 16,   1 },
	{  -245, 16,   1 },
	{   246, 16,   1 },
	{  -246, 16,   1 },
	{   247, 16,   1 },
	{  -247, 16,   1 },
	{   248, 16,   1 },
	{  -248, 16,   1 },
	{   249, 16,   1 },
	{  -249, 16,   1 },
	{   250, 16,   1 },
	{  -250, 16,   1 },
	{   251, 16,   1 },
	{  -251, 16,   1 },
	{   252, 16,   1 },
	{  -252, 16,   1 },
	{   253, 16,   1 },
	{  -253, 16,   1 },
	{   254, 16,   1 },
	{  -254, 16,   1 },
	{   255, 16,   1 },
	{  -255, 16,   1 }
};
/* }}} */
/* {{{ [fold] qc_mjpeg_lvc_decode_block(struct bitstream *bitsrc, s16 *output, int blockval) */
static inline void qc_mjpeg_lvc_decode_block(struct bitstream *bitsrc, s16 *output, int blockval)
{
	u32 b;
	u8 *p;
	int k;
	int value, skip, bits;
	struct hufftable_entry entry;
	int offset = 0;
	const u8 *shiftPtr;

	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_lvc_decode_block(bitsrc=%p, output=%p, blockval=%i)", bitsrc, output, blockval);
	b = bitsrc->b;
	k = bitsrc->k;
	p = bitsrc->p;
	memset(output, 0, 64 * sizeof(s16));
	if (blockval!=7) PDEBUG("blockval=%i",blockval);
	NEEDBITS(b,k,p);
	shiftPtr = shiftTables[shiftTblIndex[2*blockval+BITVALUE(b,1)]];
	DUMPBITS(b,k,1);
	value = BITVALUE(((signed)b),10);
	DUMPBITS(b,k,10);
	do {
		value = ((value << shiftPtr[offset]) * scaleTable[offset]) >> 14;
		output[scan_norm[offset]] = value;
		NEEDBITS(b,k,p);
		entry = hufftable[BITVALUE(b,8)];
		bits = entry.bits;
		if (bits > 8) {
			entry = hufftable[entry.value + ((b & 0x00ffffff) >> (32 - bits))];
			if (PARANOID && entry.bits!=bits) {
				PDEBUG("entry.bits!=bits shouldn't happen");
				bits = entry.bits;
			}
		}
		DUMPBITS(b,k,bits);
		skip = entry.skip;
		value = entry.value;
		offset += skip;
	} while (offset < 64);
	bitsrc->b = b;
	bitsrc->k = k;
	bitsrc->p = p;
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_lvc_decode_block() exit");
}
/* }}} */

/* {{{ [fold] struct blockorder */
struct blockorder {
	char widthPad;		/* pad width to multiple of this */
	char heightPad;		/* pad height to multiple of this */
	char uvWshift;		/* shift width by this to get width of U/V image */
	char uvHshift;		/* dito for height */
	char blockWidth[2];	/* width of a block for each pass*/
	char subblockCount[2];	/* number of sub block in a block for each pass */
	u32 subblockMap[2];
};
static const struct blockorder order_I420 = {
	32, 16, 1, 1,
	{ 32, 16 }, { 4, 4 },
	{ 0x00, 0x90 }
};
#if 0
static const struct blockorder order_L422 = {
	16, 16, 1, 0, 
	{ 16, 16 }, { 4, 4 },
	{ 0x90, 0x90 }
};

static const struct blockorder order_L410 = {
	64, 16, 2, 1,
	{ 32, 64 }, { 4, 12 },
	{ 0x00, 0x909000 }
};
#endif
/* }}} */
/* {{{ [fold] qc_mjpeg_lvc_decode() */
/* Decode given compressed image to YUV image. Return error code if bad data */
static int qc_mjpeg_lvc_decode(u8 *outY, u8 *outU, u8 *outV,
	u8 *input, u32 length, unsigned int width, unsigned int height)
{
	struct bitstream stream;
	const struct blockorder *blkorder;

	unsigned int blockx, blocky;
	unsigned int pass, subblock, blockval = 0;
	unsigned int blocknr = 0;
	unsigned int uvWidth;

	s16 blockbuffer[64];

	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_lvc_decode(outY=%p, outU=%p, outV=%p)", outY, outU, outV);
	stream.b   = 0;
	stream.k   = 16;
	stream.p   = input;
	stream.end = input+length;

	blkorder = &order_I420;		/* Select compression type */

	uvWidth = (width >> blkorder->uvWshift);

	if ((width & (blkorder->widthPad - 1)) || (height & (blkorder->heightPad - 1))) {
		PDEBUG("something's wrong");
		return -EILSEQ;
	}

	for (blocky=0; blocky<height; blocky+=blkorder->heightPad) {
		for (pass = 0; pass < 2; pass++) {
			int blockwidth    = blkorder->blockWidth[pass];
			int subblockcount = blkorder->subblockCount[pass];
			u32 map = blkorder->subblockMap[pass];
			for (blockx=0; blockx<width; blockx+=blockwidth) {
				u32 subblkmap = map;
				for (subblock=0; subblock<subblockcount; subblock++) {
					if ((blocknr++ & 3) == 0) {
						u32 b = stream.b;
						int k = stream.k;
						u8 *p = stream.p;

						NEEDBITS(b, k, p);

						/* Make sure from time to time that we don't read
						 * far too much.  I hope it is okay to read a bit
						 * beyond the end
						 */
						if (p > stream.end) {
							PDEBUG("p>stream.end");
							return -EILSEQ;
						}

						blockval = BITVALUE(b, 4);
						DUMPBITS(b,k,4);
						stream.b = b;
						stream.k = k;
						stream.p = p;
					}
					qc_mjpeg_lvc_decode_block(&stream, blockbuffer, blockval);
					blockbuffer[0] += 1024;
					switch (subblkmap & 3) {
					case 0:
						qc_mjpeg_idct(blockbuffer, outY, width);
						outY += 8;
						break;
					case 1:
						qc_mjpeg_idct(blockbuffer, outU, uvWidth);
						outU += 8;
						break;
					case 2:
						qc_mjpeg_idct(blockbuffer, outV, uvWidth);
						outV += 8;
						break;
					}
					subblkmap >>= 2;
				} /* for (subblock = 0; subblock < subblockcount; subblock++) */
			} /* for (blockx = 0; blockx < width; blockx += blockwidth) */
			outY += 7 * width;
			if (map) {
				outU += 7 * uvWidth;
				outV += 7 * uvWidth;
			}
		} /* for (pass = 0; pass < 2; pass++) */

		/* next block starts at next 4 byte boundary */
		stream.p -= (16 - stream.k) >> 3;  /* push back unread bits */
		stream.p += (input - stream.p) & 3;
		stream.k = 16;
		stream.b = 0;
	} /* for (blocky=0; blocky<height; blocky+=blkorder->heightPad) */

	if (stream.p != stream.end) {
		PDEBUG("stream.p != stream.end");
		return -EILSEQ;
	}
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_lvc_decode() done");
	return 0;
}
/* }}} */

/* }}} */
/* {{{ [fold] **** qc_mjpeg:         Motion JPEG decoding main routines *************************** */

static const int qc_mjpeg_width  = 320;		/* Size of the compressed image */
static const int qc_mjpeg_height = 240;

/* {{{ [fold] qc_mjpeg_decode() */
/* Decode and uncompress given data, return error code if failure
 * src = points to compressed bitstream data
 * src_len = compressed data length in bytes
 * dst = decompressed image will be stored here, size 320x240 x bytes per pixel (2-4)
 */
int qc_mjpeg_decode(struct qc_mjpeg_data *md, unsigned char *src, int src_len, unsigned char *dst)
{
	int r;
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_decode(src_len=%i,W=%i,H=%i,depth=%i)",src_len,qc_mjpeg_width,qc_mjpeg_height,md->depth);
	IDEBUG_TEST(*md);
	if (src_len >= 100000) {
		PDEBUG("long frame, length=%i", src_len);
		return -EILSEQ;
	}
	r = qc_mjpeg_lvc_decode(md->encY, md->encU, md->encV, src, src_len, qc_mjpeg_width, qc_mjpeg_height);
	if (r<0) {
		PRINTK(KERN_ERR,"frame corrupted, len=%i",src_len);
		return r;
	}
	qc_mjpeg_yuv2rgb(md, dst, md->encY, md->encU, md->encV, 
		qc_mjpeg_width, qc_mjpeg_height, 		/* Image size */
		qc_mjpeg_width * ((md->depth+1)/8),		/* RGB stride */
		qc_mjpeg_width,					/* Y stride */
		qc_mjpeg_width/2);				/* U and V stride */
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_decode() done");
	return 0;
}
/* }}} */
/* {{{ [fold] qc_mjpeg_init(struct qc_mjpeg_data *md, int depth, Bool tobgr) */
/* Initialize Motion JPEG decompression.
 * depth = bit depth of the decoded image, either 15=16,24 or 32
 * tobgr = use blue in the lowest address (red otherwise)
 */
int qc_mjpeg_init(struct qc_mjpeg_data *md, int depth, Bool tobgr)
{
	int r = -ENOMEM;
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_init(depth=%i)",depth);
	md->depth = depth;
	/* Temporary buffers used for decoding the image (FIXME:too big for stack?) */
	/* Note: originally this allocated one extra byte for encY/U/V. I removed that. */
	md->encY = kmalloc(qc_mjpeg_width*qc_mjpeg_height, GFP_KERNEL);	
	if (!md->encY) goto fail1;
	md->encU = kmalloc(qc_mjpeg_width*qc_mjpeg_height/4, GFP_KERNEL);
	if (!md->encU) goto fail2;
	md->encV = kmalloc(qc_mjpeg_width*qc_mjpeg_height/4, GFP_KERNEL);
	if (!md->encV) goto fail3;
	if ((r=qc_mjpeg_yuv2rgb_init(md, depth, tobgr ? MODE_BGR : MODE_RGB))<0) goto fail4;
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_init() done");
	IDEBUG_INIT(*md);
	return 0;

fail4:	kfree(md->encV);
fail3:	kfree(md->encU);
fail2:	kfree(md->encY);
fail1:	PDEBUG("failed qc_mjpeg_init() = %i", r);
	POISON(*md);
	return r;
}
/* }}} */
/* {{{ [fold] qc_mjpeg_exit(struct qc_mjpeg_data *md) */
/* Free up resources allocated for image decompression */
void qc_mjpeg_exit(struct qc_mjpeg_data *md)
{
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_exit()");
	IDEBUG_TEST(*md);
	qc_mjpeg_yuv2rgb_exit(md);
	kfree(md->encV);
	kfree(md->encU);
	kfree(md->encY);
	POISON(md->encV);
	POISON(md->encU);
	POISON(md->encY);
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_exit() done");
	IDEBUG_EXIT(*md);
}
/* }}} */

/* }}} */

#else /* COMPRESS=0 */
int qc_mjpeg_decode(struct qc_mjpeg_data *md, unsigned char *src, int src_len, unsigned char *dst) { return -ENXIO; }
int qc_mjpeg_init(struct qc_mjpeg_data *md, int depth, Bool tobgr) { return -ENXIO; }
void qc_mjpeg_exit(struct qc_mjpeg_data *md) { }
#endif

/* End of file */