img.c

freescale MX21 0V9650摄像头驱动程序

#include "myhead.h"

//#define IMGDEBUG

#define FFPLAY
#ifdef FFPLAY
extern int debug_no;
#endif

#define FB_HEIGHT 320
#define FB_WIDTH 240

//outbuf should has a size of inw*inh*3/2 (in bytes)
//currently, inw=104, inh=80
//Note: order: y, v, u
int YCbCrtoRGB444(char *py, int inw, int inh, char *outbuf)
{
	//      U--Cb, V--Cr
	int y1, u1, v1;
	int r1, g1, b1;
	int y2, u2, v2;
	int r2, g2, b2;
	int wu, wv;
	char *pu, *pv;

	int i, j;


	wu = inw / 2;
	wv = inw / 2;
	pv = py + 104 * 80;
	pu = pv + 52 * 40;

#ifdef IMGDEBUG
	printf("inw:%d, inh:%d\n", inw, inh);
	printf("py:%p, pu:%p pv:%p\n", py, pu, pv);
#endif

	for (j = 0; j < inh; j++) {
		for (i = 0; i < inw; i += 2) {
//#define GRAYTEST
#ifdef  GRAYTEST
			y1 = (int) (unsigned char) py[j * inw + i];
			u1 = u2 =
			    (int) (unsigned char) pu[j / 2 * wu + i / 2];
			v1 = v2 =
			    (int) (unsigned char) pv[j / 2 * wv + i / 2];
			r1 = b1 = g1 = y1 >> 4;

			y2 = (int) (unsigned char) py[j * inw + i + 1];
			//Can be omitted
			u2 = (int) (unsigned char) pu[j / 2 * wu +
						      (i + 1) / 2];
			v2 = (int) (unsigned char) pv[j / 2 * wv +
						      (i + 1) / 2];
			r2 = b2 = g2 = y2 >> 4;

			outbuf[j * inw / 2 * 3 + i / 2 * 3] = r1 << 4 | g1;
			outbuf[j * inw / 2 * 3 + i / 2 * 3 + 1] =
			    b1 << 4 | r2;
			outbuf[j * inw / 2 * 3 + i / 2 * 3 + 2] =
			    g2 << 4 | b2;
			if (i == 0 && j == 0) {
				printf("i:0, j:0\n");
				printf("y1:(%d), u1:(%d), v1:(%d)\n", y1,
				       u1, v1);
				printf("r1:(%d), g1:(%d), b1:(%d)\n", r1,
				       g1, b1);
			}
			if (i == 22 && j == 0) {
				printf("i:33, j:0\n");
				printf("y1:(%d), u1:(%d), v1:(%d)\n", y1,
				       u1, v1);
				printf("r1:(%d), g1:(%d), b1:(%d)\n", r1,
				       g1, b1);
				printf("y2:(%d), u2:(%d), v2:(%d)\n", y2,
				       u2, v2);
				printf("r2:(%d), g2:(%d), b2:(%d)\n", r2,
				       g2, b2);
			}
#endif
			y1 = (int) (unsigned char) py[j * inw + i];
			u1 = u2 =
			    (int) (unsigned char) pu[j / 2 * wu + i / 2];
			v1 = v2 =
			    (int) (unsigned char) pv[j / 2 * wv + i / 2];
			y2 = (int) (unsigned char) py[j * inw + i + 1];
#if 0
			u2 = (int) (unsigned char) pu[j / 2 * wu +
						      (i + 1) / 2];
			v2 = (int) (unsigned char) pv[j / 2 * wv +
						      (i + 1) / 2];
#endif
			r1 = y1 + (v1 - 128) * 1402 / 1000;
			g1 = y1 - (u1 - 128) * 34414 / 100000 - (-128 +
								 v1) *
			    71414 / 100000;
			b1 = y1 + (u1 - 128) * 1772 / 1000;
			r2 = y2 + (v2 - 128) * 1402 / 1000;
			g2 = y2 - (u2 - 128) * 34414 / 100000 - (-128 +
								 v2) *
			    71414 / 100000;
			b2 = y2 + (u2 - 128) * 1772 / 1000;

			r1 >>= 4;
			g1 >>= 4;
			b1 >>= 4;
			r2 >>= 4;
			g2 >>= 4;
			b2 >>= 4;

			outbuf[j * inw / 2 * 3 + i / 2 * 3] = r1 << 4 | g1;
			outbuf[j * inw / 2 * 3 + i / 2 * 3 + 1] =
			    b1 << 4 | r2;
			outbuf[j * inw / 2 * 3 + i / 2 * 3 + 2] =
			    g2 << 4 | b2;
#if 0
			if (i == 0 && j == 0) {
				printf("i:0, j:0\n");
				printf("y1:(%d), u1:(%d), v1:(%d)\n", y1,
				       u1, v1);
				printf("r1:(%d), g1:(%d), b1:(%d)\n", r1,
				       g1, b1);
			}
			if (i == 22 && j == 0) {
				printf("i:33, j:0\n");
				printf("y1:(%d), u1:(%d), v1:(%d)\n", y1,
				       u1, v1);
				printf("r1:(%d), g1:(%d), b1:(%d)\n", r1,
				       g1, b1);
				printf("y2:(%d), u2:(%d), v2:(%d)\n", y2,
				       u2, v2);
				printf("r2:(%d), g2:(%d), b2:(%d)\n", r2,
				       g2, b2);
			}
#endif
		}
	}
	return 0;
}


//outbuf should has a size of inw*inh(in shorts)
int YCbCrtoRGB565(char *py, char *pu, char *pv, int inw, int inh,
		  short *outbuf)
{
	//      U--Cb, V--Cr
	int y, u, v;
	int r, g, b;
	unsigned short r16, g16, b16;
	int wu, wv;

	int i, j;

#ifdef IMGDEBUG
	printf("data0:%p, data1:%p, data2:%p\n", py, pu, pv);
	printf("inw:%d, inh:%d\n", inw, inh);
#endif

	wu = inw / 2;
	wv = inw / 2;

	for (j = 0; j < inh; j++) {
		for (i = 0; i < inw; i++) {
			y = (int) (unsigned char) py[j * inw + i];
//#define GRAYTEST
#ifdef  GRAYTEST
			g = y >> 2;
			r = b = g >> 1;
#else
			u = (int) (unsigned char) pu[j / 2 * wu + i / 2];
			v = (int) (unsigned char) pv[j / 2 * wv + i / 2];

			r = y + (v - 128) * 1402 / 1000;
			g = y - (u - 128) * 34414 / 100000 - (-128 +
							      v) * 71414 /
			    100000;
			b = y + (u - 128) * 1772 / 1000;
#ifdef IMGDEBUG
			if (i == 0 && j == 0) {
				printf("y:%x(%d), u:%x(%d), v:%x(%d)\n", y,
				       y, u, u, v, v);
				printf("r:%x(%d), g:%x(%d), b:%x(%d)\n", r,
				       r, g, g, b, b);
			}
#endif
			r16 = ((unsigned short) r) >> 3;
			g16 = ((unsigned short) g) >> 2;
			b16 = ((unsigned short) b) >> 3;
#endif
			outbuf[j * inw + i] = r16 << 11 | g16 << 5 | b16;
		}
	}
	return 0;
}

void RGB565toYCbCr(char *r_inbuf, char *r_outbuf, int width, int height)
{
	unsigned char y, u, v;
	int r, g, b;

	unsigned short *inbuf = (unsigned short *) r_inbuf;
	char *data0, *data1, *data2;
	int i, j, index, pixel_size;
	unsigned short src;


	pixel_size = width * height;
	data0 = r_outbuf;
	data1 = data0 + pixel_size;
	data2 = data1 + pixel_size / 4;

	//printf("data0:%p, data1:%p, data2:%p\n", data0, data1, data2);
	for (j = 0; j < height; j++) {
		for (i = 0; i < width; i++) {
			index = j * width + i;
			src = inbuf[index];
			//expand rgb to 8bit 
			r = src >> 11 << 3;
			g = (0x07E0 & src) >> 5 << 2;
			b = (0x001F & src) << 3;

			/*compute YUV value */
			y = ((r * 299 + g * 587 + b * 114) / 1000);
			u = (-1687 * r - 3313 * g + 5000 * b) / 10000 +
			    128;
			v = (r * 5000 - g * 4187 - b * 813) / 10000 + 128;

			//y = ((r*299+g*587+ b*114)/1000);
			//u=(b-y)*127/226+128;
			//v=(r-y)*127/179+128;

			//y=((r*313524)>>20)+((g*615514)>>20)+((b*119538)>>20);
			//u=(((b-y)*589244)>>20)+128;
			//v=(((r-y)*743962)>>20)+128;

#if 0
			if (i == 0 && j == 0) {
				printf("r:%x, g:%x, b:%x\n", r, g, b);
				printf("y:%x, u:%x, v:%x\n", y, u, v);
			}
#endif
			data0[index] = y;
			if (i % 2 == 0 && j % 2 == 0) {
				data1[j / 2 * width / 2 + i / 2] = u;
				data2[j / 2 * width / 2 + i / 2] = v;
			}
		}		//i
	}			//j
}

//return gray scale of a RGB565 word, in 4 bits
unsigned int RGB565toGray(unsigned short src)
{
	unsigned short t, r, g, b;
	unsigned int RMove = 12, GMove = 7, BMove = 1;
	unsigned short GMask = 0x07E0;
	unsigned short BMask = 0x001F;

	r = src >> RMove;
	g = (GMask & src) >> GMove;
	b = (BMask & src) >> BMove;

	t = (r * 3 + g * 6 + b) / 10;
	return t;
}

//      Transform RGB565 to RGB444
void RGB565ToRGB444(char *r_ibuf, char *r_obuf, long size)
{
	int i, j;
	unsigned char *ibuf, *obuf;
	unsigned char r1, g1, b1, r2, g2, b2;

	ibuf = r_ibuf;
	obuf = r_obuf;
	//Color transform from 565 to 444
	//2 RGB565 pixels is 4 bytes,
	//and after transformed to 2 RGB444 pixels is 3 bytes
	for (i = 0, j = 0; i < size; i += 4, j += 3) {
		//first 565 pixel
		r1 = (ibuf[i + 1] & 0xF8) >> 3;
		g1 = ((ibuf[i + 1] & 0x07)) << 3 | ((ibuf[i] & 0xE0)) >> 5;
		b1 = ibuf[i] & 0x1F;

		//second 565 pixel
		r2 = (ibuf[i + 3] & 0xF8) >> 3;
		g2 = ((ibuf[i + 3] & 0x07)) << 3 |
		    ((ibuf[i + 2] & 0xE0)) >> 5;
		b2 = ibuf[i + 2] & 0x1F;

		//scale 565 to 444 
		r1 >>= 1;	//5 to 4
		g1 >>= 2;	//6 to 4
		b1 >>= 1;	//5 to 4
		r2 >>= 1;	//5 to 4
		g2 >>= 2;	//6 to 4
		b2 >>= 1;	//5 to 4

		//Combine r, g, b parts together
		obuf[j] = (r1 << 4) | g1;
		obuf[j + 1] = (b1 << 4) | r2;
		obuf[j + 2] = (g2 << 4) | b2;
	}
	return;
}


//      Transform RGB565 to RGB444 gray scale
void RGB565ToGray444(char *r_ibuf, char *r_obuf, long size)
{
	int i, j;
	char *ibuf, *obuf;
	unsigned int gray1, gray2;

	ibuf = r_ibuf;
	obuf = r_obuf;
	//Color transform from 565 to 444
	//2 RGB565 pixels is 4 bytes,
	//and after transformed to 2 RGB444 pixels is 3 bytes
	for (i = 0, j = 0; i < size; i += 4, j += 3) {
		//first 565 pixel
		gray1 = RGB565toGray(*(unsigned short *) (ibuf + i));

		//second 565 pixel
		gray2 = RGB565toGray(*(unsigned short *) (ibuf + i + 2));

		//Combine r, g, b parts together
		obuf[j] = (unsigned char) (gray1 << 4 | gray1);
		obuf[j + 1] = (unsigned char) (gray1 << 4 | gray2);
		obuf[j + 2] = (unsigned char) (gray2 << 4 | gray2);
	}