qc-formats.c

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

/* Start of file */

/* {{{ [fold] Comments */
/*
 * qc-usb, linux V4L driver for the Logitech QuickCam family
 *
 * qc-formats.c - converts color formats
 *
 * Copyright (c) 2001 Jean-Frederic Clere (RGB->YUV conversion)
 *               2001 Aaron Holtzman <[e-mail]> (YUV->RGB conversion, from mpeg2dec)
 *               2003 Tuukka Toivonen (Bayer->RGB conversion)
 *
 * This program 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.
 *
 * This program 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
#include <asm/byteorder.h>

#ifndef v4l2_fourcc
/*  Four-character-code (FOURCC) */
#define v4l2_fourcc(a,b,c,d)\
        (((__u32)(a)<<0)|((__u32)(b)<<8)|((__u32)(c)<<16)|((__u32)(d)<<24))
#endif

/* {{{ [fold] **** qc_yuv:  Start of RGB to YUV conversion functions  ******************** */

/* {{{ [fold] qc_yuv_interp[256][8] */
static signed short qc_yuv_interp[256][8] = {
{0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0},{0,1,0,0,0,1,0,0},{0,1,0,0,0,1,-1,0},
{1,2,0,0,-1,2,-1,0},{1,2,0,0,-1,2,-2,0},{1,3,0,-1,-1,3,-2,0},{2,3,0,-1,-2,3,-2,0},
{2,4,0,-1,-2,4,-3,0},{2,5,1,-1,-2,4,-3,0},{2,5,1,-1,-3,5,-4,0},{3,6,1,-1,-3,5,-4,0},
{3,6,1,-2,-3,6,-5,0},{3,7,1,-2,-4,6,-5,-1},{4,7,1,-2,-4,7,-5,-1},{4,8,1,-2,-4,7,-6,-1},
{4,9,1,-2,-5,8,-6,-1},{5,9,1,-2,-5,8,-7,-1},{5,10,2,-3,-5,9,-7,-1},{5,10,2,-3,-6,9,-7,-1},
{5,11,2,-3,-6,10,-8,-1},{6,11,2,-3,-6,10,-8,-1},{6,12,2,-3,-7,11,-9,-1},{6,13,2,-3,-7,11,-9,-1},
{7,13,2,-4,-7,12,-10,-1},{7,14,2,-4,-8,12,-10,-2},{7,14,2,-4,-8,13,-10,-2},{8,15,3,-4,-8,13,-11,-2},
{8,15,3,-4,-9,14,-11,-2},{8,16,3,-4,-9,14,-12,-2},{8,17,3,-5,-9,15,-12,-2},{9,17,3,-5,-10,15,-12,-2},
{9,18,3,-5,-10,16,-13,-2},{9,18,3,-5,-10,16,-13,-2},{10,19,3,-5,-11,17,-14,-2},{10,19,3,-5,-11,17,-14,-2},
{10,20,4,-6,-11,18,-15,-2},{11,20,4,-6,-12,18,-15,-3},{11,21,4,-6,-12,19,-15,-3},{11,22,4,-6,-12,19,-16,-3},
{11,22,4,-6,-13,20,-16,-3},{12,23,4,-6,-13,20,-17,-3},{12,23,4,-7,-13,21,-17,-3},{12,24,4,-7,-14,21,-18,-3},
{13,24,5,-7,-14,22,-18,-3},{13,25,5,-7,-14,22,-18,-3},{13,26,5,-7,-15,23,-19,-3},{14,26,5,-7,-15,23,-19,-3},
{14,27,5,-8,-15,24,-20,-3},{14,27,5,-8,-16,24,-20,-3},{14,28,5,-8,-16,25,-20,-4},{15,28,5,-8,-16,25,-21,-4},
{15,29,5,-8,-17,26,-21,-4},{15,30,6,-8,-17,26,-22,-4},{16,30,6,-9,-17,27,-22,-4},{16,31,6,-9,-18,27,-23,-4},
{16,31,6,-9,-18,28,-23,-4},{17,32,6,-9,-18,28,-23,-4},{17,32,6,-9,-19,29,-24,-4},{17,33,6,-9,-19,29,-24,-4},
{17,34,6,-10,-19,30,-25,-4},{18,34,6,-10,-20,30,-25,-4},{18,35,7,-10,-20,31,-25,-5},{18,35,7,-10,-20,31,-26,-5},
{19,36,7,-10,-21,32,-26,-5},{19,36,7,-10,-21,32,-27,-5},{19,37,7,-11,-21,33,-27,-5},{20,37,7,-11,-22,33,-28,-5},
{20,38,7,-11,-22,34,-28,-5},{20,39,7,-11,-22,34,-28,-5},{20,39,7,-11,-23,35,-29,-5},{21,40,8,-11,-23,35,-29,-5},
{21,40,8,-12,-23,36,-30,-5},{21,41,8,-12,-24,36,-30,-5},{22,41,8,-12,-24,37,-30,-6},{22,42,8,-12,-24,37,-31,-6},
{22,43,8,-12,-25,38,-31,-6},{23,43,8,-12,-25,38,-32,-6},{23,44,8,-13,-25,39,-32,-6},{23,44,9,-13,-26,39,-33,-6},
{23,45,9,-13,-26,40,-33,-6},{24,45,9,-13,-26,40,-33,-6},{24,46,9,-13,-27,41,-34,-6},{24,47,9,-14,-27,41,-34,-6},
{25,47,9,-14,-27,42,-35,-6},{25,48,9,-14,-28,42,-35,-6},{25,48,9,-14,-28,43,-36,-6},{26,49,9,-14,-28,43,-36,-7},
{26,49,10,-14,-29,44,-36,-7},{26,50,10,-15,-29,44,-37,-7},{26,51,10,-15,-29,45,-37,-7},{27,51,10,-15,-30,45,-38,-7},
{27,52,10,-15,-30,46,-38,-7},{27,52,10,-15,-30,46,-38,-7},{28,53,10,-15,-31,47,-39,-7},{28,53,10,-16,-31,47,-39,-7},
{28,54,10,-16,-31,48,-40,-7},{29,54,11,-16,-32,48,-40,-7},{29,55,11,-16,-32,49,-41,-7},{29,56,11,-16,-32,49,-41,-8},
{29,56,11,-16,-33,50,-41,-8},{30,57,11,-17,-33,50,-42,-8},{30,57,11,-17,-33,51,-42,-8},{30,58,11,-17,-34,51,-43,-8},
{31,58,11,-17,-34,52,-43,-8},{31,59,11,-17,-34,52,-43,-8},{31,60,12,-17,-35,53,-44,-8},{31,60,12,-18,-35,53,-44,-8},
{32,61,12,-18,-35,54,-45,-8},{32,61,12,-18,-36,54,-45,-8},{32,62,12,-18,-36,55,-46,-8},{33,62,12,-18,-36,55,-46,-9},
{33,63,12,-18,-37,56,-46,-9},{33,64,12,-19,-37,56,-47,-9},{34,64,12,-19,-37,57,-47,-9},{34,65,13,-19,-38,57,-48,-9},
{34,65,13,-19,-38,58,-48,-9},{34,66,13,-19,-38,58,-48,-9},{35,66,13,-19,-39,59,-49,-9},{35,67,13,-20,-39,59,-49,-9},
{35,68,13,-20,-39,60,-50,-9},{36,68,13,-20,-40,60,-50,-9},{36,69,13,-20,-40,61,-51,-9},{36,69,14,-20,-40,61,-51,-9},
{37,70,14,-20,-41,62,-51,-10},{37,70,14,-21,-41,62,-52,-10},{37,71,14,-21,-41,63,-52,-10},{37,72,14,-21,-42,63,-53,-10},
{38,72,14,-21,-42,64,-53,-10},{38,73,14,-21,-42,64,-54,-10},{38,73,14,-21,-43,65,-54,-10},{39,74,14,-22,-43,65,-54,-10},
{39,74,15,-22,-43,66,-55,-10},{39,75,15,-22,-44,66,-55,-10},{40,75,15,-22,-44,67,-56,-10},{40,76,15,-22,-44,67,-56,-10},
{40,77,15,-22,-45,68,-56,-11},{40,77,15,-23,-45,68,-57,-11},{41,78,15,-23,-45,69,-57,-11},{41,78,15,-23,-46,69,-58,-11},
{41,79,15,-23,-46,70,-58,-11},{42,79,16,-23,-46,70,-59,-11},{42,80,16,-23,-47,71,-59,-11},{42,81,16,-24,-47,71,-59,-11},
{43,81,16,-24,-47,72,-60,-11},{43,82,16,-24,-48,72,-60,-11},{43,82,16,-24,-48,73,-61,-11},{43,83,16,-24,-48,73,-61,-11},
{44,83,16,-24,-49,74,-61,-12},{44,84,16,-25,-49,74,-62,-12},{44,85,17,-25,-49,75,-62,-12},{45,85,17,-25,-50,75,-63,-12},
{45,86,17,-25,-50,76,-63,-12},{45,86,17,-25,-50,76,-64,-12},{46,87,17,-25,-51,77,-64,-12},{46,87,17,-26,-51,77,-64,-12},
{46,88,17,-26,-51,78,-65,-12},{46,89,17,-26,-52,78,-65,-12},{47,89,18,-26,-52,79,-66,-12},{47,90,18,-26,-52,79,-66,-12},
{47,90,18,-26,-53,80,-66,-13},{48,91,18,-27,-53,80,-67,-13},{48,91,18,-27,-53,81,-67,-13},{48,92,18,-27,-54,81,-68,-13},
{49,92,18,-27,-54,82,-68,-13},{49,93,18,-27,-54,82,-69,-13},{49,94,18,-28,-54,83,-69,-13},{49,94,19,-28,-55,83,-69,-13},
{50,95,19,-28,-55,84,-70,-13},{50,95,19,-28,-55,84,-70,-13},{50,96,19,-28,-56,85,-71,-13},{51,96,19,-28,-56,85,-71,-13},
{51,97,19,-29,-56,86,-72,-13},{51,98,19,-29,-57,86,-72,-14},{52,98,19,-29,-57,87,-72,-14},{52,99,19,-29,-57,87,-73,-14},
{52,99,20,-29,-58,88,-73,-14},{52,100,20,-29,-58,88,-74,-14},{53,100,20,-30,-58,89,-74,-14},{53,101,20,-30,-59,89,-74,-14},
{53,102,20,-30,-59,90,-75,-14},{54,102,20,-30,-59,90,-75,-14},{54,103,20,-30,-60,91,-76,-14},{54,103,20,-30,-60,91,-76,-14},
{55,104,20,-31,-60,92,-77,-14},{55,104,21,-31,-61,92,-77,-15},{55,105,21,-31,-61,93,-77,-15},{55,106,21,-31,-61,93,-78,-15},
{56,106,21,-31,-62,94,-78,-15},{56,107,21,-31,-62,94,-79,-15},{56,107,21,-32,-62,95,-79,-15},{57,108,21,-32,-63,95,-79,-15},
{57,108,21,-32,-63,96,-80,-15},{57,109,22,-32,-63,96,-80,-15},{58,109,22,-32,-64,97,-81,-15},{58,110,22,-32,-64,97,-81,-15},
{58,111,22,-33,-64,98,-82,-15},{58,111,22,-33,-65,98,-82,-16},{59,112,22,-33,-65,99,-82,-16},{59,112,22,-33,-65,99,-83,-16},
{59,113,22,-33,-66,100,-83,-16},{60,113,22,-33,-66,100,-84,-16},{60,114,23,-34,-66,101,-84,-16},{60,115,23,-34,-67,101,-84,-16},
{60,115,23,-34,-67,102,-85,-16},{61,116,23,-34,-67,102,-85,-16},{61,116,23,-34,-68,103,-86,-16},{61,117,23,-34,-68,103,-86,-16},
{62,117,23,-35,-68,104,-87,-16},{62,118,23,-35,-69,104,-87,-16},{62,119,23,-35,-69,105,-87,-17},{63,119,24,-35,-69,105,-88,-17},
{63,120,24,-35,-70,106,-88,-17},{63,120,24,-35,-70,106,-89,-17},{63,121,24,-36,-70,107,-89,-17},{64,121,24,-36,-71,107,-90,-17},
{64,122,24,-36,-71,108,-90,-17},{64,123,24,-36,-71,108,-90,-17},{65,123,24,-36,-72,109,-91,-17},{65,124,24,-36,-72,109,-91,-17},
{65,124,25,-37,-72,110,-92,-17},{66,125,25,-37,-73,110,-92,-17},{66,125,25,-37,-73,111,-92,-18},{66,126,25,-37,-73,111,-93,-18},
{66,127,25,-37,-74,112,-93,-18},{67,127,25,-37,-74,112,-94,-18},{67,128,25,-38,-74,113,-94,-18},{67,128,25,-38,-75,113,-95,-18},
{68,129,25,-38,-75,114,-95,-18},{68,129,26,-38,-75,114,-95,-18},{68,130,26,-38,-76,115,-96,-18},{69,130,26,-38,-76,115,-96,-18},
{69,131,26,-39,-76,116,-97,-18},{69,132,26,-39,-77,116,-97,-18},{69,132,26,-39,-77,117,-97,-19},{70,133,26,-39,-77,117,-98,-19},
{70,133,26,-39,-78,118,-98,-19},{70,134,27,-39,-78,118,-99,-19},{71,134,27,-40,-78,119,-99,-19},{71,135,27,-40,-79,119,-100,-19},
{71,136,27,-40,-79,120,-100,-19},{72,136,27,-40,-79,120,-100,-19},{72,137,27,-40,-80,121,-101,-19},{72,137,27,-40,-80,121,-101,-19},
{72,138,27,-41,-80,122,-102,-19},{73,138,27,-41,-81,122,-102,-19},{73,139,28,-41,-81,123,-103,-19},{73,140,28,-41,-81,123,-103,-20},
{74,140,28,-41,-82,124,-103,-20},{74,141,28,-42,-82,124,-104,-20},{74,141,28,-42,-82,125,-104,-20},{75,142,28,-42,-83,125,-105,-20},
{75,142,28,-42,-83,126,-105,-20},{75,143,28,-42,-83,126,-105,-20},{75,144,28,-42,-84,127,-106,-20},{76,144,29,-43,-84,127,-106,-20}};
/* }}} */

/* {{{ [fold] qc_yuv_toplanar(unsigned char *rgb24frame, int length, int format) */
/*
 * Convert to planar formats
 * The input is an YCbYCr format.
 * Input: len : 2/3 maxi.
 *  | YUYV          | free |
 *  | 2/3           | 1/3  |
 * 1th conversion:
 *  | YY  | free    | U|V  |
 *  | 1/3 | 1/3     | 1/3  |
 * 2d conversion:
 *  | YY  | U  | V  | free |
 *  | 1/3 | 1/6|1/6 | 1/3  |
 * That the Y422P conversion.
 */
static int qc_yuv_toplanar(unsigned char *rgb24frame, int length, int format)
{
	unsigned char *ptr;
	int n = 0, l = 0, i;
	unsigned char *cr;
	unsigned char *cb;
	unsigned char *crptr, *cbptr;
	int mode = 0;
	
	l = length/2;
	switch(format) {
		case VIDEO_PALETTE_YUV411P:
			n = length/8;
			mode = 1;
			break;
		case VIDEO_PALETTE_YUV422P:
			n = length/4;
			break;
	}

	ptr = rgb24frame;
	crptr = &rgb24frame[length];
	cr = crptr;
	cbptr = &rgb24frame[length+n];
	cb = cbptr;
	if (qcdebug&QC_DEBUGLOGIC) PDEBUG("qc_yuv_toplanar: %d (%d + 2 * %d)",length, l, n);

	/* separate Y , U and V */
	for (i=0; i<length; i+=2) {
		*ptr++ = rgb24frame[i];
		if (mode) {
			if ((i/2)%4==3) {
				*cbptr++ = rgb24frame[i+1];
				*crptr++ = rgb24frame[i-1];
			}
		}
		else {
			if ((i/2)%2)
				*cbptr++ = rgb24frame[i+1];
			else
				*crptr++ = rgb24frame[i+1];
		}
	}

	/* copy the UV plans after the Y */
	memcpy(&rgb24frame[l], cr, n);
	memcpy(&rgb24frame[l+n], cb, n);
	// That is useless! frame->scanlength = length/2+ (2 * n);
	return length;
}
/* }}} */
/* {{{ [fold] qc_yuv_rgb2yuv(unsigned char *rgb24frame, int length, int format) */
/*
 * Convert the RGB24 image to YUV422.
 * return the size of the converted frame
 */
int qc_yuv_rgb2yuv(unsigned char *rgb24frame, int length, int format)
{
	int i;
	int l;			/* Data length */
	short Y;
	short U;
	short V;
	unsigned char *ptr;
	unsigned short w;

	//if (qcdebug&QC_DEBUGLOGIC)PDEBUG("qc_yuv_rgb2yuv(frame=%p,length=%i,palette=%s)",rgb24frame,length,qc_fmt_getname(format));
	ptr = rgb24frame;
	l = 0;

	if (format==VIDEO_PALETTE_RGB32) {
		/* we need to convert in reverse so as to not overwrite ourselves */
		for (ptr=ptr+length*4/3,i=length-3; i>=0; i-=3) {
			*--ptr = 0;
			*--ptr = (unsigned char) rgb24frame[i + 2];
			*--ptr = (unsigned char) rgb24frame[i + 1];
			*--ptr = (unsigned char) rgb24frame[i + 0];
			l += 4;
		}
	} else
	for (i=0; i<length; i+=3) {
		Y = qc_yuv_interp[rgb24frame[i]][0] + qc_yuv_interp[rgb24frame[i+1]][1] + qc_yuv_interp[rgb24frame[i+2]][2];
		U = qc_yuv_interp[rgb24frame[i]][3] + qc_yuv_interp[rgb24frame[i+1]][4] + qc_yuv_interp[rgb24frame[i+2]][5];
		V = qc_yuv_interp[rgb24frame[i]][5] + qc_yuv_interp[rgb24frame[i+1]][6] + qc_yuv_interp[rgb24frame[i+2]][7];

		/* color/brightness could be arranged here */

		Y = CLIP(Y, 0,255);
		U = CLIP(U, -127,127);
		V = CLIP(V, -127,127);

		switch(format) {
		case VIDEO_PALETTE_GREY:
			*ptr++ = (219 * Y)/255 + 16;
			l++;
			break;
		case VIDEO_PALETTE_YUV411:
			// Not yet supported.
			break;
		case VIDEO_PALETTE_YUV411P:
		case VIDEO_PALETTE_YUV422:
		case VIDEO_PALETTE_YUV422P:
			*ptr++ = (219 * Y)/255 + 16;
			l++;
			if ((i/3)%2)
				*ptr = (112 * U)/127 + 128; /* Cb */
			else
				*ptr = (112 * V)/127 + 128; /* Cr */
			ptr++;
			l++;
			break;
		case VIDEO_PALETTE_RGB565:
			/* take top five bits and pack while switch RGB with BGR */
			/* FIXME: do we need to take into account of non-x86 byte ordering? */
			w = ((unsigned short)
				((unsigned short) (rgb24frame[i + 2] & 0xF8) << (11 - 3)) |
				((unsigned short) (rgb24frame[i + 1] & 0xFC) << (5 - 2)) |
				((unsigned short) (rgb24frame[i + 0] >> 3)));
			*ptr++ = (unsigned char) (w & 0xFF);
			*ptr++ = (unsigned char) (w >> 8);
			l += 2;
			break;
		case VIDEO_PALETTE_RGB555:
			w = ((unsigned short)
				((unsigned short) (rgb24frame[i + 2] & 0xf8) << (10 - 3)) |
				((unsigned short) (rgb24frame[i + 1] & 0xf8) << (5 - 3)) |
				((unsigned short) (rgb24frame[i + 0] >> 3)));
			*ptr++ = (unsigned char) (w & 0xFF);
			*ptr++ = (unsigned char) (w >> 8);
			l += 2;
			break;
		default:
			*ptr++ = (219 * Y)/255 + 16;
			l++;
			*ptr = (112 * U)/127 + 128; /* Cb */
			ptr++;
			l++;
			*ptr = (112 * V)/127 + 128; /* Cr */
			ptr++;
			l++;
		}
	}
	if (format >= VIDEO_PALETTE_PLANAR) {
		length = qc_yuv_toplanar(rgb24frame, l, format);
	} else {
		length = l;
	}
	return length;
}
/* }}} */

/* }}} */
/* {{{ [fold] **** qc_imag: Start of image processing functions ************************** */

/* {{{ [fold] qc_imag_bayer_midvalue(char *bay, int bay_line, int columns, int rows) */
/* Compute and return the average pixel intensity from the bayer data. 
 * The result can be used for automatic software exposure/gain control.
 * bay = points to the bayer image data (upper left pixel is green)
 * bay_line = bytes between the beginnings of two consecutive rows
 * columns, rows = bayer image size (both must be even)
 */
static unsigned char qc_imag_bayer_midvalue(unsigned char *bay, int bay_line, 
	unsigned int columns, unsigned int rows)
{
	static const unsigned int stepsize = 8;		/* Larger = faster and less accurate */
	unsigned char *cur_bay;
	unsigned int sum = 0;