ov511.c

Linux内核源代码 为压缩文件 是<<Linux内核>>一书中的源代码

	default:
		err("Invalid sensor");
		return -EINVAL;
	}

	/* Bit 5 of COM C register varies with sensor */ 
	if (ov511->sensor == SEN_OV6620) {
		if (width > 176 && height > 144) {  /* CIF */
			ov511_i2c_write(dev, 0x14, 0x04);
			hwscale = 1;
			vwscale = 1;  /* The datasheet says 0; it's wrong */
			hwsize = 352;
			vwsize = 288;
		} else {			    /* QCIF */
			ov511_i2c_write(dev, 0x14, 0x24);
			hwsize = 176;
			vwsize = 144;
		}
	}
	else {
		if (width > 320 && height > 240) {  /* VGA */
			ov511_i2c_write(dev, 0x14, 0x04);
			hwscale = 2;
			vwscale = 1;
			hwsize = 640;
			vwsize = 480;
		} else {			    /* QVGA */
			ov511_i2c_write(dev, 0x14, 0x24);
			hwscale = 1;
			hwsize = 320;
			vwsize = 240;
		}	
	}

	/* FIXME! - This needs to be changed to support 160x120 and 6620!!! */
	if (sub_flag) {
		ov511_i2c_write(dev, 0x17, hwsbase+(ov511->subx>>hwscale));
		ov511_i2c_write(dev, 0x18, hwebase+((ov511->subx+ov511->subw)>>hwscale));
		ov511_i2c_write(dev, 0x19, vwsbase+(ov511->suby>>vwscale));
		ov511_i2c_write(dev, 0x1a, vwebase+((ov511->suby+ov511->subh)>>vwscale));
	} else {
		ov511_i2c_write(dev, 0x17, hwsbase);
		ov511_i2c_write(dev, 0x18, hwebase + (hwsize>>hwscale));
		ov511_i2c_write(dev, 0x19, vwsbase);
		ov511_i2c_write(dev, 0x1a, vwebase + (vwsize>>vwscale));
	}

	for (i = 0; mlist[i].width; i++) {
		int lncnt, pxcnt, clock;

		if (width != mlist[i].width || height != mlist[i].height)
			continue;

		if (!mlist[i].color && mode != VIDEO_PALETTE_GREY)
			continue;

		/* Here I'm assuming that snapshot size == image size.
		 * I hope that's always true. --claudio
		 */
		pxcnt = sub_flag ? (ov511->subw >> 3) - 1 : mlist[i].pxcnt;
		lncnt = sub_flag ? (ov511->subh >> 3) - 1 : mlist[i].lncnt;

		ov511_reg_write(dev, 0x12, pxcnt);
		ov511_reg_write(dev, 0x13, lncnt);
		ov511_reg_write(dev, 0x14, mlist[i].pxdv);
		ov511_reg_write(dev, 0x15, mlist[i].lndv);
		ov511_reg_write(dev, 0x18, mlist[i].m420);

		/* Snapshot additions */
		ov511_reg_write(dev, 0x1a, pxcnt);
		ov511_reg_write(dev, 0x1b, lncnt);
                ov511_reg_write(dev, 0x1c, mlist[i].pxdv);
                ov511_reg_write(dev, 0x1d, mlist[i].lndv);

		/* Calculate and set the clock divisor */
		clock = ((sub_flag ? ov511->subw * ov511->subh : width * height)
			* (mlist[i].color ? 3 : 2) / 2) / 66000;
#if 0
		clock *= cams;
#endif
		ov511_i2c_write(dev, 0x11, clock);

		/* We only have code to convert GBR -> RGB24 */
		if ((mode == VIDEO_PALETTE_RGB24) && sensor_gbr)
			ov511_i2c_write(dev, 0x12, mlist[i].common_A | (testpat?0x0a:0x08));
		else
			ov511_i2c_write(dev, 0x12, mlist[i].common_A | (testpat?0x02:0x00));

		/* 7620/6620 don't have register 0x35, so play it safe */
		if (ov511->sensor == SEN_OV7610 ||
		    ov511->sensor == SEN_OV7620AE)
			ov511_i2c_write(dev, 0x35, mlist[i].common_L);

		break;
	}

	if (compress) {
		ov511_reg_write(dev, 0x78, 0x03); // Turn on Y compression
		ov511_reg_write(dev, 0x79, 0x00); // Disable LUTs
	}

	if (ov511_restart(ov511->dev) < 0)
		return -EIO;

	if (mlist[i].width == 0) {
		err("Unknown mode (%d, %d): %d", width, height, mode);
		return -EINVAL;
	}

#ifdef OV511_DEBUG
	if (debug >= 5)
		ov511_dump_i2c_regs(dev);
#endif

	return 0;
}

/**********************************************************************
 *
 * Color correction functions
 *
 **********************************************************************/

/*
 * Turn a YUV4:2:0 block into an RGB block
 *
 * Video4Linux seems to use the blue, green, red channel
 * order convention-- rgb[0] is blue, rgb[1] is green, rgb[2] is red.
 *
 * Color space conversion coefficients taken from the excellent
 * http://www.inforamp.net/~poynton/ColorFAQ.html
 * In his terminology, this is a CCIR 601.1 YCbCr -> RGB.
 * Y values are given for all 4 pixels, but the U (Pb)
 * and V (Pr) are assumed constant over the 2x2 block.
 *
 * To avoid floating point arithmetic, the color conversion
 * coefficients are scaled into 16.16 fixed-point integers.
 * They were determined as follows:
 *
 *	double brightness = 1.0;  (0->black; 1->full scale) 
 *	double saturation = 1.0;  (0->greyscale; 1->full color)
 *	double fixScale = brightness * 256 * 256;
 *	int rvScale = (int)(1.402 * saturation * fixScale);
 *	int guScale = (int)(-0.344136 * saturation * fixScale);
 *	int gvScale = (int)(-0.714136 * saturation * fixScale);
 *	int buScale = (int)(1.772 * saturation * fixScale);
 *	int yScale = (int)(fixScale);	
 */

/* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
#define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))

static inline void
ov511_move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v, 
	int rowPixels, unsigned char * rgb, int bits)
{
	const int rvScale = 91881;
	const int guScale = -22553;
	const int gvScale = -46801;
	const int buScale = 116129;
	const int yScale  = 65536;
	int r, g, b;

	g = guScale * u + gvScale * v;
	if (force_rgb) {
		r = buScale * u;
		b = rvScale * v;
	} else {
		r = rvScale * v;
		b = buScale * u;
	}

	yTL *= yScale; yTR *= yScale;
	yBL *= yScale; yBR *= yScale;

	if (bits == 24) {
		/* Write out top two pixels */
		rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL); rgb[2] = LIMIT(r+yTL);
		rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR); rgb[5] = LIMIT(r+yTR);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 3 * rowPixels;
		rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL); rgb[2] = LIMIT(r+yBL);
		rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR); rgb[5] = LIMIT(r+yBR);
	} else if (bits == 16) {
		/* Write out top two pixels */
		rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F) | ((LIMIT(g+yTL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07) | (LIMIT(r+yTL) & 0xF8);

		rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F) | ((LIMIT(g+yTR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07) | (LIMIT(r+yTR) & 0xF8);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 2 * rowPixels;

		rgb[0] = ((LIMIT(b+yBL) >> 3) & 0x1F) | ((LIMIT(g+yBL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yBL) >> 5) & 0x07) | (LIMIT(r+yBL) & 0xF8);

		rgb[2] = ((LIMIT(b+yBR) >> 3) & 0x1F) | ((LIMIT(g+yBR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yBR) >> 5) & 0x07) | (LIMIT(r+yBR) & 0xF8);
	}
}

/*
 * For a 640x480 YUV4:2:0 images, data shows up in 1200 384 byte segments.
 * The first 64 bytes of each segment are U, the next 64 are V.  The U and
 * V are arranged as follows:
 *
 *      0  1 ...  7
 *      8  9 ... 15
 *           ...   
 *     56 57 ... 63
 *
 * U and V are shipped at half resolution (1 U,V sample -> one 2x2 block).
 *
 * The next 256 bytes are full resolution Y data and represent 4 squares
 * of 8x8 pixels as follows:
 *
 *      0  1 ...  7    64  65 ...  71   ...  192 193 ... 199
 *      8  9 ... 15    72  73 ...  79        200 201 ... 207
 *           ...              ...                    ...
 *     56 57 ... 63   120 121 ... 127   ...  248 249 ... 255
 *
 * Note that the U and V data in one segment represents a 16 x 16 pixel
 * area, but the Y data represents a 32 x 8 pixel area.
 *
 * If dumppix module param is set, _parse_data just dumps the incoming segments,
 * verbatim, in order, into the frame. When used with vidcat -f ppm -s 640x480
 * this puts the data on the standard output and can be analyzed with the
 * parseppm.c utility I wrote.  That's a much faster way for figuring out how
 * this data is scrambled.
 */

#define HDIV 8
#define WDIV (256/HDIV)

static void
ov511_parse_gbr422_to_rgb24(unsigned char *pIn0, unsigned char *pOut0,
		       int iOutY, int iOutUV, int iHalf, int iWidth)
{
	int k, l, m;
	unsigned char *pIn;
	unsigned char *pOut, *pOut1;

	pIn = pIn0;
	pOut = pOut0 + iOutUV + (force_rgb ? 2 : 0);
		
	for (k = 0; k < 8; k++) {
		pOut1 = pOut;
		for (l = 0; l < 8; l++) {
			*pOut1 = *(pOut1 + 3) = *(pOut1 + iWidth*3) =
				*(pOut1 + iWidth*3 + 3) = *pIn++;
			pOut1 += 6;
		}
		pOut += iWidth*3*2;
	}

	pIn = pIn0 + 64;
	pOut = pOut0 + iOutUV + (force_rgb ? 0 : 2);
	for (k = 0; k < 8; k++) {
		pOut1 = pOut;
		for (l = 0; l < 8; l++) {
			*pOut1 = *(pOut1 + 3) = *(pOut1 + iWidth*3) =
				*(pOut1 + iWidth*3 + 3) = *pIn++;
			pOut1 += 6;
		}
		pOut += iWidth*3*2;
	}

	pIn = pIn0 + 128;
	pOut = pOut0 + iOutY + 1;
	for (k = 0; k < 4; k++) {
		pOut1 = pOut;
		for (l = 0; l < 8; l++) {
			for (m = 0; m < 8; m++) {
				*pOut1 = *pIn++;
				pOut1 += 3;
			}
			pOut1 += (iWidth - 8) * 3;
		}
		pOut += 8 * 3;
	}
}

static void
ov511_parse_yuv420_to_rgb(unsigned char *pIn0, unsigned char *pOut0,
		       int iOutY, int iOutUV, int iHalf, int iWidth, int bits)
{
	int k, l, m;
	int bytes = bits >> 3;
	unsigned char *pIn;
	unsigned char *pOut, *pOut1;

	/* Just copy the Y's if in the first stripe */
	if (!iHalf) {
		pIn = pIn0 + 128;
		pOut = pOut0 + iOutY;
		for (k = 0; k < 4; k++) {
			pOut1 = pOut;
			for (l = 0; l < 8; l++) {
				for (m = 0; m < 8; m++) {
					*pOut1 = *pIn++;
					pOut1 += bytes;
				}
				pOut1 += (iWidth - 8) * bytes;
			}
			pOut += 8 * bytes;
		}
	}

	/* Use the first half of VUs to calculate value */
	pIn = pIn0;
	pOut = pOut0 + iOutUV;
	for (l = 0; l < 4; l++) {
		for (m=0; m<8; m++) {
			int y00 = *(pOut);
			int y01 = *(pOut+bytes);
			int y10 = *(pOut+iWidth*bytes);
			int y11 = *(pOut+iWidth*bytes+bytes);
			int v   = *(pIn+64) - 128;
			int u   = *pIn++ - 128;
			ov511_move_420_block(y00, y01, y10, y11, u, v, iWidth,
				pOut, bits);
			pOut += 2 * bytes;
		}
		pOut += (iWidth*2 - 16) * bytes;
	}

	/* Just copy the other UV rows */
	for (l = 0; l < 4; l++) {
		for (m = 0; m < 8; m++) {
			*pOut++ = *(pIn + 64);
			*pOut = *pIn++;
			pOut += 2 * bytes - 1;
		}
		pOut += (iWidth*2 - 16) * bytes;
	}

	/* Calculate values if it's the second half */
	if (iHalf) {
		pIn = pIn0 + 128;
		pOut = pOut0 + iOutY;
		for (k = 0; k < 4; k++) {
			pOut1 = pOut;
			for (l=0; l<4; l++) {
				for (m=0; m<4; m++) {
					int y10 = *(pIn+8);
					int y00 = *pIn++;
					int y11 = *(pIn+8);
					int y01 = *pIn++;
					int v   = *pOut1 - 128;
					int u   = *(pOut1+1) - 128;
					ov511_move_420_block(y00, y01, y10,
						y11, u, v, iWidth, pOut1, bits);
					pOut1 += 2 * bytes;
				}
				pOut1 += (iWidth*2 - 8) * bytes;
				pIn += 8;
			}
			pOut += 8 * bytes;
		}
	}
}

static void
ov511_dumppix(unsigned char *pIn0, unsigned char *pOut0,
	      int iOutY, int iOutUV, int iHalf, int iWidth)
{
	int i, j, k;
	unsigned char *pIn, *pOut, *pOut1;

	switch (dumppix) {
	case 1: /* Just dump YUV data straight out for debug */
		pOut0 += iOutY;
		for (i = 0; i < HDIV; i++) {
			for (j = 0; j < WDIV; j++) {
				*pOut0++ = *pIn0++;
				*pOut0++ = *pIn0++;
				*pOut0++ = *pIn0++;
			}
			pOut0 += (iWidth - WDIV) * 3;
		}
		break;
	case 2: /* This converts the Y data to "black-and-white" RGB data */
		/* Useful for experimenting with compression */
		pIn = pIn0 + 128;
		pOut = pOut0 + iOutY;
		for (i = 0; i < 4; i++) {
			pOut1 = pOut;
			for (j = 0; j < 8; j++) {
				for (k = 0; k < 8; k++) {
					*pOut1++ = *pIn;
					*pOut1++ = *pIn;
					*pOut1++ = *pIn++;
				}
				pOut1 += (iWidth - 8) * 3;
			}
			pOut += 8 * 3;
		}
		break;
	case 3: /* This will dump only the Y channel data stream as-is */
		pIn = pIn0 + 128;
		pOut = pOut0 + output_offset;
		for (i = 0; i < 256; i++) {
			*pOut++ = *pIn;
			*pOut++ = *pIn;
			*pOut++ = *pIn++;
			output_offset += 3;
		}
		break;
	} /* End switch (dumppix) */
}

/* This converts YUV420 segments to YUYV */
static void
ov511_parse_data_yuv422(unsigned char *pIn0, unsigned char *pOut0,
		        int iOutY, int iOutUV, int iWidth)
{
	int k, l, m;
	unsigned char *pIn, *pOut, *pOut1;

	pIn = pIn0 + 128;
	pOut = pOut0 + iOutY;
	for (k = 0; k < 4; k++) {
		pOut1 = pOut;
		for (l = 0; l < 8; l++) {
			for (m = 0; m < 8; m++) {
				*pOut1 = (*pIn++);
				pOut1 += 2;
			}
			pOut1 += (iWidth - 8) * 2;
		}
		pOut += 8 * 2;
	}

	pIn = pIn0;
	pOut = pOut0 + iOutUV + 1;
	for (l = 0; l < 8; l++) {
		for (m=0; m<8; m++) {
			int v   = *(pIn+64);
			int u   = *pIn++;
			
			*pOut = u;
			*(pOut+2) = v;
			*(pOut+iWidth) = u;
			*(pOut+iWidth+2) = v;
			pOut += 4;
		}
		pOut += (iWidth*4 - 32);
	}
}

static void
ov511_parse_data_yuv420(unsigned char *pIn0, unsigned char *pOut0,
		        int iOutY, int iOutUV, int iWidth, int iHeight)
{
	int k, l, m;
	unsigned char *pIn;
	unsigned char *pOut, *pOut1;
	unsigned a = iWidth * iHeight;
	unsigned w = iWidth / 2;

	pIn = pIn0;
	pOut = pOut0 + iOutUV + a;
	for (k = 0; k < 8; k++) {
		pOut1 = pOut;
		for (l = 0; l < 8; l++) *pOut1++ = *pIn++;
		pOut += w;
	}

	pIn = pIn0 + 64;
	pOut = pOut0 + iOutUV + a + a/4;
	for (k = 0; k < 8; k++) {
		pOut1 = pOut;
		for (l = 0; l < 8; l++) *pOut1++ = *pIn++;
		pOut += w;
	}

	pIn = pIn0 + 128;
	pOut = pOut0 + iOutY;
	for (k = 0; k < 4; k++) {
		pOut1 = pOut;
		for (l = 0; l < 8; l++) {
			for (m = 0; m < 8; m++)
				*pOut1++ =*pIn++;
			pOut1 += iWidth - 8;
		}
		pOut += 8;
	}
}

static void
ov511_parse_data_yuv422p(unsigned char *pIn0, unsigned char *pOut0,
		       int iOutY, int iOutUV, int iWidth, int iHeight)
{
	int k, l, m;
	unsigned char *pIn;
	unsigned char *pOut, *pOut1;
	unsigned a = iWidth * iHeight;
	unsigned w = iWidth / 2;

	pIn = pIn0;
	pOut = pOut0 + iOutUV + a;
	for (k = 0; k < 8; k++) {
		pOut1 = pOut;