qc-mjpeg.c

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

/* Start of file */

/* {{{ [fold] Comments */
/*
 * MJPEG decompression routines are from mpeg2dec,
 * Copyright (C) 1999-2001 Aaron Holtzman <[e-mail]>
 * Modified by Tuukka Toivonen and Jochen Hoenicke.
 *
 * Portions of this code are from the MPEG software simulation group
 * idct implementation. This code will be replaced with a new
 * implementation soon.
 *
 * The MJPEG routines are from mpeg2dec, a free MPEG-2 video stream decoder.
 *
 * mpeg2dec is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * mpeg2dec is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
/* }}} */

#ifdef NOKERNEL
#include "quickcam.h"
#else
#include <linux/quickcam.h>
#endif

#ifdef __KERNEL__	/* show.c will include this file directly into compilation for userspace */
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,0)
#include <linux/slab.h>
#else
#include <linux/malloc.h>
#endif
#endif /* __KERNEL__ */

#if COMPRESS
/* {{{ [fold] **** qc_mjpeg_yuv2rgb: MJPEG decoding: YUV to RGB conversion routines *************** */

/* {{{ [fold] Macros */
#define MODE_RGB  1
#define MODE_BGR  2

#define RGB(i)							\
	U = pu[i];						\
	V = pv[i];						\
	r = md->table_rV[V];					\
	g = (void *)(((u8 *)md->table_gU[U]) + md->table_gV[V]);\
	b = md->table_bU[U];

#define DST1(i)					\
	Y = py_1[2*i];				\
	dst_1[2*i] = r[Y] + g[Y] + b[Y];	\
	Y = py_1[2*i+1];			\
	dst_1[2*i+1] = r[Y] + g[Y] + b[Y];

#define DST2(i)					\
	Y = py_2[2*i];				\
	dst_2[2*i] = r[Y] + g[Y] + b[Y];	\
	Y = py_2[2*i+1];			\
	dst_2[2*i+1] = r[Y] + g[Y] + b[Y];

#define DST1RGB(i)							\
	Y = py_1[2*i];							\
	dst_1[6*i] = r[Y]; dst_1[6*i+1] = g[Y]; dst_1[6*i+2] = b[Y];	\
	Y = py_1[2*i+1];						\
	dst_1[6*i+3] = r[Y]; dst_1[6*i+4] = g[Y]; dst_1[6*i+5] = b[Y];

#define DST2RGB(i)							\
	Y = py_2[2*i];							\
	dst_2[6*i] = r[Y]; dst_2[6*i+1] = g[Y]; dst_2[6*i+2] = b[Y];	\
	Y = py_2[2*i+1];						\
	dst_2[6*i+3] = r[Y]; dst_2[6*i+4] = g[Y]; dst_2[6*i+5] = b[Y];

#define DST1BGR(i)							\
	Y = py_1[2*i];							\
	dst_1[6*i] = b[Y]; dst_1[6*i+1] = g[Y]; dst_1[6*i+2] = r[Y];	\
	Y = py_1[2*i+1];						\
	dst_1[6*i+3] = b[Y]; dst_1[6*i+4] = g[Y]; dst_1[6*i+5] = r[Y];

#define DST2BGR(i)							\
	Y = py_2[2*i];							\
	dst_2[6*i] = b[Y]; dst_2[6*i+1] = g[Y]; dst_2[6*i+2] = r[Y];	\
	Y = py_2[2*i+1];						\
	dst_2[6*i+3] = b[Y]; dst_2[6*i+4] = g[Y]; dst_2[6*i+5] = r[Y];
/* }}} */

/* {{{ [fold] qc_mjpeg_yuv2rgb_32() */
static void qc_mjpeg_yuv2rgb_32(struct qc_mjpeg_data *md, u8 *py_1, u8 *py_2, u8 *pu, u8 *pv,
	void *_dst_1, void *_dst_2, int width)
{
	int U, V, Y;
	u32 *r, *g, *b;
	u32 *dst_1, *dst_2;
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_32(md=%p, py_1=%p, py_2=%p, pu=%p, pv=%p, _dst_1=%p, _dst_2=%p, width=%i",md,py_1,py_2,pu,pv,_dst_1,_dst_2,width);
	width >>= 3;
	dst_1 = _dst_1;
	dst_2 = _dst_2;

	do {
		RGB(0);
		DST1(0);
		DST2(0);

		RGB(1);
		DST2(1);
		DST1(1);

		RGB(2);
		DST1(2);
		DST2(2);

		RGB(3);
		DST2(3);
		DST1(3);

		pu += 4;
		pv += 4;
		py_1 += 8;
		py_2 += 8;
		dst_1 += 8;
		dst_2 += 8;
	} while (--width);
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_32() done");
}
/* }}} */
/* {{{ [fold] qc_mjpeg_yuv2rgb_24rgb() */
/* This is very near from the yuv2rgb_32 code */
static void qc_mjpeg_yuv2rgb_24rgb(struct qc_mjpeg_data *md, u8 *py_1, u8 *py_2, u8 *pu, u8 *pv, 
	void *_dst_1, void *_dst_2, int width)
{
	int U, V, Y;
	u8 *r, *g, *b;
	u8 *dst_1, *dst_2;
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_24rgb(md=%p, py_1=%p, py_2=%p, pu=%p, pv=%p, _dst_1=%p, _dst_2=%p, width=%i",md,py_1,py_2,pu,pv,_dst_1,_dst_2,width);

	width >>= 3;
	dst_1 = _dst_1;
	dst_2 = _dst_2;

	do {
		RGB(0);
		DST1RGB(0);
		DST2RGB(0);

		RGB(1);
		DST2RGB(1);
		DST1RGB(1);

		RGB(2);
		DST1RGB(2);
		DST2RGB(2);

		RGB(3);
		DST2RGB(3);
		DST1RGB(3);

		pu += 4;
		pv += 4;
		py_1 += 8;
		py_2 += 8;
		dst_1 += 24;
		dst_2 += 24;
	} while (--width);
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_24rgb() done");
}
/* }}} */
/* {{{ [fold] qc_mjpeg_yuv2rgb_24bgr() */
/* only trivial mods from yuv2rgb_24rgb */
static void qc_mjpeg_yuv2rgb_24bgr(struct qc_mjpeg_data *md, u8 *py_1, u8 *py_2, u8 *pu, u8 *pv, 
	void *_dst_1, void *_dst_2, int width)
{
	int U, V, Y;
	u8 *r, *g, *b;
	u8 *dst_1, *dst_2;

	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_24bgr(md=%p, py_1=%p, py_2=%p, pu=%p, pv=%p, _dst_1=%p, _dst_2=%p, width=%i",md,py_1,py_2,pu,pv,_dst_1,_dst_2,width);
	width >>= 3;
	dst_1 = _dst_1;
	dst_2 = _dst_2;

	do {
		RGB(0);
		DST1BGR(0);
		DST2BGR(0);

		RGB(1);
		DST2BGR(1);
		DST1BGR(1);

		RGB(2);
		DST1BGR(2);
		DST2BGR(2);

		RGB(3);
		DST2BGR(3);
		DST1BGR(3);

		pu += 4;
		pv += 4;
		py_1 += 8;
		py_2 += 8;
		dst_1 += 24;
		dst_2 += 24;
	} while (--width);
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_24bgr() done");
}
/* }}} */
/* {{{ [fold] qc_mjpeg_yuv2rgb_16() */
/* This is exactly the same code as yuv2rgb_32 except for the types of */
/* r, g, b, dst_1, dst_2 */
static void qc_mjpeg_yuv2rgb_16(struct qc_mjpeg_data *md, u8 *py_1, u8 *py_2, u8 *pu, u8 *pv, 
	void *_dst_1, void *_dst_2, int width)
{
	int U, V, Y;
	u16 *r, *g, *b;
	u16 *dst_1, *dst_2;

	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_16(md=%p, py_1=%p, py_2=%p, pu=%p, pv=%p, _dst_1=%p, _dst_2=%p, width=%i",md,py_1,py_2,pu,pv,_dst_1,_dst_2,width);
	width >>= 3;
	dst_1 = _dst_1;
	dst_2 = _dst_2;

	do {
		RGB(0);
		DST1(0);
		DST2(0);

		RGB(1);
		DST2(1);
		DST1(1);

		RGB(2);
		DST1(2);
		DST2(2);

		RGB(3);
		DST2(3);
		DST1(3);

		pu += 4;
		pv += 4;
		py_1 += 8;
		py_2 += 8;
		dst_1 += 8;
		dst_2 += 8;
	} while (--width);
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_16() done");
}
/* }}} */
/* {{{ [fold] qc_mjpeg_yuv2rgb() */
/* Convert YUV image to RGB */
static void qc_mjpeg_yuv2rgb(struct qc_mjpeg_data *md, void *dst, u8 *py, u8 *pu, u8 *pv, 
	int width, int height, int rgb_stride, int y_stride, int uv_stride)
{
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb(md=%p, dst=%p, py=%p, pu=%p, pv=%p, width=%i, height=%i, rgb_stride=%i, y_stride=%i, uv_stride=%i",md,dst,py,pu,pv,width,height,rgb_stride,y_stride,uv_stride);
	height >>= 1;
	do {
		md->yuv2rgb_func(md, py, py + y_stride, pu, pv, dst, ((u8 *)dst) + rgb_stride, width);
		py += 2 * y_stride;
		pu += uv_stride;
		pv += uv_stride;
		dst = ((u8 *)dst) + 2 * rgb_stride;
	} while (--height);
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb() done");
}
/* }}} */

static const u32 matrix_coefficients = 6;
static const s32 Inverse_Table_6_9[8][4] = {
	{ 117504, 138453, 13954, 34903 },	/* 0: no sequence_display_extension */
	{ 117504, 138453, 13954, 34903 },	/* 1: ITU-R Rec. 709 (1990) */
	{ 104597, 132201, 25675, 53279 },	/* 2: unspecified */
	{ 104597, 132201, 25675, 53279 },	/* 3: reserved */
	{ 104448, 132798, 24759, 53109 },	/* 4: FCC */
	{ 104597, 132201, 25675, 53279 },	/* 5: ITU-R Rec. 624-4 System B, G */
	{ 104597, 132201, 25675, 53279 },	/* 6: SMPTE 170M */
	{ 117579, 136230, 16907, 35559 } 	/* 7: SMPTE 240M (1987) */
};

/* {{{ [fold] div_round(int dividend, int divisor) */
static int div_round(int dividend, int divisor)
{
	if (dividend > 0) 
		return (dividend + (divisor>>1)) / divisor;
	else
		return -((-dividend + (divisor>>1)) / divisor);
}
/* }}} */
/* {{{ [fold] qc_mjpeg_yuv2rgb_init(struct qc_mjpeg_data *md, int bpp, int mode) */
/* Initialization of yuv2rgb routines. Return error code if failure */
static inline int qc_mjpeg_yuv2rgb_init(struct qc_mjpeg_data *md, int bpp, int mode)
{
	static const int table_Y_size = 1024;
	u8 *table_Y;
	int i, ret = -ENOMEM;
	int entry_size = 0;
	void *table_r = NULL, *table_g = NULL, *table_b = NULL;
	int crv = Inverse_Table_6_9[matrix_coefficients][0];
	int cbu = Inverse_Table_6_9[matrix_coefficients][1];
	int cgu = -Inverse_Table_6_9[matrix_coefficients][2];
	int cgv = -Inverse_Table_6_9[matrix_coefficients][3];
	u32 *table_32;
	u16 *table_16;
	u8 *table_8;

	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_init(md=%p, bpp=%i, mode=%i)",md,bpp,mode);
	table_Y = kmalloc(table_Y_size,GFP_KERNEL);	/* Allocate with kmalloc(), it might not fit into stack */
	if (table_Y==NULL) return -ENOMEM;

	for (i=0; i<1024; i++) {
		int j;
		j = (76309 * (i - 384 - 16) + 32768) >> 16;
		j = (j < 0) ? 0 : ((j > 255) ? 255 : j);
		table_Y[i] = j;
	}

	switch (bpp) {
	case 32:
		md->yuv2rgb_func = qc_mjpeg_yuv2rgb_32;
		table_32 = md->table = kmalloc((197 + 2*682 + 256 + 132) * sizeof(u32), GFP_KERNEL);	/* 0..1948 x 4 */
		if (!md->table) goto fail;
		entry_size = sizeof(u32);
		table_r = table_32 + 197;		/* R: -197..1751 */
		table_b = table_32 + 197 + 685;		/* B: -882..1066 */
		table_g = table_32 + 197 + 2*682;	/* G: -1561..387 */
		for (i=-197; i<256+197; i++)		/* Ri = -197...452 */
			((u32 *) table_r)[i] = table_Y[i+384] << ((mode==MODE_RGB) ? 16 : 0);
		for (i=-132; i<256+132; i++)		/* Gi = -132...387 */
			((u32 *) table_g)[i] = table_Y[i+384] << 8;
		for (i=-232; i<256+232; i++)		/* Bi = -232...487 */
			((u32 *) table_b)[i] = table_Y[i+384] << ((mode==MODE_RGB) ? 0 : 16);
		break;
	case 24:
		md->yuv2rgb_func = (mode==MODE_RGB) ? qc_mjpeg_yuv2rgb_24rgb : qc_mjpeg_yuv2rgb_24bgr;
		table_8 = md->table = kmalloc((256 + 2*232) * sizeof(u8), GFP_KERNEL);			/* 0..719 x 1 */
		if (!md->table) goto fail;
		entry_size = sizeof(u8);
		table_r = table_g = table_b = table_8 + 232;	/* -232..487 */
		for (i=-232; i<256+232; i++)			/* i = -232..487 */
			((u8 *)table_b)[i] = table_Y[i+384];
		break;
	case 15:
	case 16:
		md->yuv2rgb_func = qc_mjpeg_yuv2rgb_16;
		table_16 = md->table = kmalloc((197 + 2*682 + 256 + 132) * sizeof(u16), GFP_KERNEL);	/* 0..1948 x 2 */
		if (!md->table) goto fail;
		entry_size = sizeof(u16);
		table_r = table_16 + 197;		/* R: -197..1751 */
		table_b = table_16 + 197 + 685;		/* B: -882..1066 */
		table_g = table_16 + 197 + 2*682;	/* G: -1561..387 */
		for (i=-197; i<256+197; i++) {		/* Ri = -197..452 */
			int j = table_Y[i+384] >> 3;
			if (mode == MODE_RGB) j <<= ((bpp==16) ? 11 : 10);
			((u16 *)table_r)[i] = j;
		}
		for (i=-132; i<256+132; i++) {		/* Gi = -132..387 */
			int j = table_Y[i+384] >> ((bpp==16) ? 2 : 3);
			((u16 *)table_g)[i] = j << 5;
		}
		for (i=-232; i<256+232; i++) {		/* Bi = -232..487 */
			int j = table_Y[i+384] >> 3;
			if (mode == MODE_BGR) j <<= ((bpp==16) ? 11 : 10);
			((u16 *)table_b)[i] = j;
		}
		break;
	default:
		PDEBUG("%i bpp not supported by yuv2rgb", bpp);
		ret = -EINVAL;
		goto fail;
	}
	for (i=0; i<256; i++) {
		md->table_rV[i] = (((u8 *)table_r) + entry_size * div_round(crv * (i-128), 76309));
		md->table_gU[i] = (((u8 *)table_g) + entry_size * div_round(cgu * (i-128), 76309));
		md->table_gV[i] = entry_size * div_round(cgv * (i-128), 76309);
		md->table_bU[i] = (((u8 *)table_b) + entry_size * div_round(cbu * (i-128), 76309));
	}
	ret = 0;
fail:	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_init()=%i done", ret);
	if (PARANOID) memset(table_Y, POISON_VAL, table_Y_size);
	kfree(table_Y);
	return ret;
}
/* }}} */
/* {{{ [fold] qc_mjpeg_yuv2rgb_exit(struct qc_mjpeg_data *md) */
static inline void qc_mjpeg_yuv2rgb_exit(struct qc_mjpeg_data *md)
{
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_exit(md=%p)",md);
	kfree(md->table);
	POISON(md->table);
	POISON(md->yuv2rgb_func);
	POISON(md->table_rV);
	POISON(md->table_gU);
	POISON(md->table_bU);
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_mjpeg_yuv2rgb_exit() done");
}
/* }}} */

/* }}} */
/* {{{ [fold] **** qc_mjpeg_idct:    MJPEG decoding: Inverse DCT routines ************************* */
/**********************************************************/
/* inverse two dimensional DCT, Chen-Wang algorithm */
/* (cf. IEEE ASSP-32, pp. 803-816, Aug. 1984) */
/* 32-bit integer arithmetic (8 bit coefficients) */
/* 11 mults, 29 adds per DCT */
/* sE, 18.8.91 */
/**********************************************************/
/* coefficients extended to 12 bit for IEEE1180-1990 */
/* compliance sE, 2.1.94 */
/**********************************************************/

/* this code assumes >> to be a two's-complement arithmetic */
/* right shift: (-2)>>1 == -1 , (-3)>>1 == -2 */

#define W1 2841		/* 2048*sqrt (2)*cos (1*pi/16) */
#define W2 2676		/* 2048*sqrt (2)*cos (2*pi/16) */
#define W3 2408		/* 2048*sqrt (2)*cos (3*pi/16) */
#define W5 1609		/* 2048*sqrt (2)*cos (5*pi/16) */
#define W6 1108		/* 2048*sqrt (2)*cos (6*pi/16) */
#define W7 565		/* 2048*sqrt (2)*cos (7*pi/16) */

/* {{{ [fold] qc_mjpeg_idct_row(s16 *block) */
/* row (horizontal) IDCT
 *
 * 7 pi 1
 * dst[k] = sum c[l] * src[l] * cos ( -- * ( k + - ) * l )
 * l=0 8 2
 *
 * where: c[0] = 128
 * c[1..7] = 128*sqrt (2)
 */
static void inline qc_mjpeg_idct_row(s16 *block)
{
	int x0, x1, x2, x3, x4, x5, x6, x7, x8;

	x1 = block[4] << 11;
	x2 = block[6];
	x3 = block[2];
	x4 = block[1];
	x5 = block[7];
	x6 = block[5];
	x7 = block[3];

	/* shortcut */
	if (! (x1 | x2 | x3 | x4 | x5 | x6 | x7)) {
		block[0] = block[1] = block[2] = block[3] = block[4] =
			block[5] = block[6] = block[7] = block[0]<<3;
		return;
	}

	x0 = (block[0] << 11) + 128; /* for proper rounding in the fourth stage */

	/* first stage */
	x8 = W7 * (x4 + x5);
	x4 = x8 + (W1 - W7) * x4;
	x5 = x8 - (W1 + W7) * x5;
	x8 = W3 * (x6 + x7);
	x6 = x8 - (W3 - W5) * x6;
	x7 = x8 - (W3 + W5) * x7;

	/* second stage */
	x8 = x0 + x1;
	x0 -= x1;
	x1 = W6 * (x3 + x2);
	x2 = x1 - (W2 + W6) * x2;
	x3 = x1 + (W2 - W6) * x3;
	x1 = x4 + x6;
	x4 -= x6;
	x6 = x5 + x7;
	x5 -= x7;

	/* third stage */
	x7 = x8 + x3;
	x8 -= x3;
	x3 = x0 + x2;
	x0 -= x2;
	x2 = (181 * (x4 + x5) + 128) >> 8;
	x4 = (181 * (x4 - x5) + 128) >> 8;