testcamera_fb2.c

基于intel xscale下的linux系统camera驱动程序

/*
	created by antiscle <[e-mail]> 2005/07/04
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <linux/fb.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#define	__user

#include "videodev.h"
#include "pxa_camera.h"

//#define	SCCB_INIT
#define USER_I2C_INIT
#define	MMAP_CAMERA
#define YCbCr_TO_RGB_TEST

struct reg_set_s {
	int val1;
	int val2;
};

#define	min(x1, x2)		(((x1)<(x2))?(x1):(x2))

#ifdef	YCbCr_TO_RGB_TEST
#include "422jpeg.h"
static void test_ycbcr_to_rgb(char *y, char *cb, char *cr, int w, int h)
{
	__u8 *buf_y, *buf_cb, *buf_cr;
	int i, j;
	
	buf_y  = (__u8 *)c422jpeg;
	buf_cb = buf_y  + 240 * 320;
	buf_cr = buf_cb + 240 * 320 / 2;
	
	for(i=0; i<min(320, h); i++) {
		for(j=0; j<min(240, w); j+=2) {
			y[j]    = buf_y[j];
			y[j+1]  = buf_y[j+1];
			cb[j/2] = buf_cb[j/2];
			cr[j/2] = buf_cr[j/2];
		}
		y  += w;
		cb += w/2;
		cr += w/2;
		buf_y  += 240;
		buf_cb += 240/2;
		buf_cr += 240/2;
	}
	
	printf("Press enter key to continue...\n");
	getchar();
}
#else
static void test_ycbcr_to_rgb(char *y, char *cb, char *cr, int w, int h) {}
#endif

#if defined(USER_I2C_INIT) || defined(SCCB_INIT)
#define	CAMERA_I2C_ADDR	0x30
#define	CAMERA_MIDH	0x1c
#define	CAMERA_MIDL	0x1d
#define	CAMERA_PIDH	0x0a
#define	CAMERA_PIDL	0x0b

#define CHIP_DELAY	0xFF
struct i2c_reg_t {
	int subaddr;
	int value;
};

static struct i2c_reg_t camera_regs[] = 
{
	{0x12, 0x80},	// Camera Soft reset. Self cleared after reset.
	{CHIP_DELAY, 20},
//change 0x11[0x81->0x80], 0x15[0x02->0x20], 0x3a[0x01->0x0d]hzh
//{0x12,0x80},
{0x11,0x80},{0x6a,0x3e},{0x3b,0x09},{0x13,0xe0},{0x01,0x80},{0x02,0x80},{0x00,0x00},{0x10,0x00},
{0x13,0xe5},{0x39,0x43},{0x38,0x12},{0x37,0x00},{0x35,0x91},{0x0e,0xa0},{0x1e,0x04},{0xA8,0x80},
{0x12,0x40},{0x04,0x00},{0x0c,0x04},{0x0d,0x80},{0x18,0xc6},{0x17,0x26},{0x32,0xad},{0x03,0x00},
{0x1a,0x3d},{0x19,0x01},{0x3f,0xa6},{0x14,0x2e},{0x15,0x20},{0x41,0x02},{0x42,0x08},{0x1b,0x00},
{0x16,0x06},{0x33,0xe2},{0x34,0xbf},{0x96,0x04},{0x3a,0x00},{0x8e,0x00},{0x3c,0x77},{0x8B,0x06},
{0x94,0x88},{0x95,0x88},{0x40,0xc1},{0x29,0x3f},{0x0f,0x42},{0x3d,0x92},{0x69,0x40},{0x5C,0xb9},
{0x5D,0x96},{0x5E,0x10},{0x59,0xc0},{0x5A,0xaf},{0x5B,0x55},{0x43,0xf0},{0x44,0x10},{0x45,0x68},
{0x46,0x96},{0x47,0x60},{0x48,0x80},{0x5F,0xe0},{0x60,0x8c},{0x61,0x20},{0xa5,0xd9},{0xa4,0x74},
{0x8d,0x02},{0x13,0xe7},{0x4f,0x3a},{0x50,0x3d},{0x51,0x03},{0x52,0x12},{0x53,0x26},{0x54,0x38},
{0x55,0x40},{0x56,0x40},{0x57,0x40},{0x58,0x0d},{0x8C,0x23},{0x3E,0x02},{0xa9,0xb8},{0xaa,0x92},
{0xab,0x0a},{0x8f,0xdf},{0x90,0x00},{0x91,0x00},{0x9f,0x00},{0xa0,0x00},{0x3A,0x0d},{0x24,0x70},
{0x25,0x64},{0x26,0xc3},{0x2a,0x00},{0x2b,0x00},{0x6c,0x40},{0x6d,0x30},{0x6e,0x4b},{0x6f,0x60},
{0x70,0x70},{0x71,0x70},{0x72,0x70},{0x73,0x70},{0x74,0x60},{0x75,0x60},{0x76,0x50},{0x77,0x48},
{0x78,0x3a},{0x79,0x2e},{0x7a,0x28},{0x7b,0x22},{0x7c,0x04},{0x7d,0x07},{0x7e,0x10},{0x7f,0x28},
{0x80,0x36},{0x81,0x44},{0x82,0x52},{0x83,0x60},{0x84,0x6c},{0x85,0x78},{0x86,0x8c},{0x87,0x9e},
{0x88,0xbb},{0x89,0xd2},{0x8a,0xe6},
//	{0x3a, 0x0d},	//
	//{0x3a, 0x1d},	//for test
	//{0x67, 'U'},	//fixed value for U
	//{0x68, 'V'},	//fixed value for V
	//{0x15, 0x12},	//PCLK reverse, VSYNC negative
	//{0x12, 0x10},	//QVGA
	//{0x04, 0x20},	//QQVGA
	//{0x15, 0x20},	//no PCLK when HREF is low
};

#define CAMERA_REGS (sizeof(camera_regs)/sizeof(camera_regs[0]))

#include <sys/time.h>
static void delay_ms(long ms)
{
/*	struct timeval tvs, tve;
	struct timezone tzs, tze;

	if(!ms)
		return;
	gettimeofday(&tvs, &tzs);
	while(1) {
		long ds, dus;
		gettimeofday(&tve, &tze);
		dus = tve.tv_usec - tvs.tv_usec;
		if(dus<0) {
			dus += 1000000;
			tve.tv_sec--;
		}
		ds = tve.tv_sec - tvs.tv_sec;
		if((ds*1000000+dus)/1000>=ms)
			break;
	}
*/
	fd_set rfds;
	struct timeval tv;

	FD_ZERO(&rfds);
	FD_SET(0, &rfds);

	tv.tv_sec = 0;
	tv.tv_usec = ms*1000;

	select(1, &rfds, NULL,  NULL, &tv);
}
#endif

#ifdef USER_I2C_INIT
static int i2c_write(int fd, __u8 reg, __u8 val)
{
	int retries;
	__u8 data[2];

	data[0] = reg;
	data[1] = val;
	for(retries=5; retries; retries--) {
		if(write(fd, data, 2)==2)
			return 0;
		delay_ms(2);
	}
	return -1;
}

static int i2c_read(int fd, __u8 reg, __u8 *val)
{
	int retries;

	for(retries=5; retries; retries--) {
		if(write(fd, &reg, 1)==1)
			if(read(fd, val, 1)==1)
				return 0;
		delay_ms(2);
	}
	return -1;
}

#define I2C_SLAVE	0x0703	/* Change slave address			*/
static int camera_i2c_init(void)
{
	int i, fd;
	__u8 id[4];

	//delay_ms(100);
	printf("open i2c device...\n");
	fd = open("/dev/i2c/0", O_RDWR);
	if(fd<0) {
		fd = open("/dev/misc/i2c", O_RDWR);
		if(fd<0) {
			printf("fail to open i2c adapter device!\n");
			return -1;
		}
	}

	if(ioctl(fd, I2C_SLAVE, CAMERA_I2C_ADDR)<0) {
		printf("fail to set i2c device slave address!\n");
		close(fd);
		return -1;
	}
	printf("set slave address to 0x%x success!\n", CAMERA_I2C_ADDR);

	if(i2c_read(fd, CAMERA_MIDH, id)||i2c_read(fd, CAMERA_MIDL, id+1)) {
		printf("fail to get camera MID!\n");
		close(fd);
		return -1;
	}
	printf("manufactory ID is 0x%04x\n", (id[0]<<8)|id[1]);

	i2c_read(fd, CAMERA_PIDH, id+2);
	i2c_read(fd, CAMERA_PIDL, id+3);
	printf("product ID is 0x%04x\n", (id[2]<<8)|id[3]);

	for(i=0; i<CAMERA_REGS; i++) {
		if(camera_regs[i].subaddr==CHIP_DELAY)
			delay_ms(camera_regs[i].value);
		else if(i2c_write(fd, camera_regs[i].subaddr, camera_regs[i].value))
			printf("write subaddr 0x%x fail!\n", camera_regs[i].subaddr);
		//delay_ms(2);
		//printf("%d\n", i);
	}
	
	close(fd);
	return 0;
}
#else
#define	camera_i2c_init()	0
#endif

#ifdef SCCB_INIT
static int sccb_init(int fd)
{
	int i;
	__u16 val;
	__u8 id[4];
	
	val = CAMERA_MIDH<<8;
	if(ioctl(fd, WCAM_VIDIOCGCAMREG, &val)<0) {
		printf("get camera register fail!\n");
		return -1;
	}
	id[0] = val;
	val = CAMERA_MIDL<<8;
	if(ioctl(fd, WCAM_VIDIOCGCAMREG, &val)<0) {
		printf("get camera register fail!\n");
		return -1;
	}
	id[1] = val;
	printf("manufactory ID is 0x%04x\n", (id[0]<<8)|id[1]);
	
	val = CAMERA_PIDH<<8;
	if(ioctl(fd, WCAM_VIDIOCGCAMREG, &val)<0) {
		printf("get camera register fail!\n");
		return -1;
	}
	id[2] = val;
	val = CAMERA_PIDL<<8;
	if(ioctl(fd, WCAM_VIDIOCGCAMREG, &val)<0) {
		printf("get camera register fail!\n");
		return -1;
	}
	id[3] = val;
	printf("product ID is 0x%04x\n", (id[2]<<8)|id[3]);

	for(i=0; i<CAMERA_REGS; i++) {
		if(camera_regs[i].subaddr==CHIP_DELAY)
			delay_ms(camera_regs[i].value);
		else {
			val = (camera_regs[i].subaddr<<8)|camera_regs[i].value;
			if(ioctl(fd, WCAM_VIDIOCSCAMREG, &val)<0)
				printf("write subaddr 0x%x fail!\n", camera_regs[i].subaddr);
		}
	}

	return 0;
}
#else
#define	sccb_init(fd)		0
#endif


/*
 * [in]   dev, bpp, format, xpos, ypos, xres, yres
 * [out]  map, yoff, ylen, cboff, cblen, croff, crlen , pitch
 *
 * format:  0x2 = YCbCr 444 Planar, 0x3 = YCbCr 422 Planar, 0x4 = YCbCr 420 Planar
 * pitch :  Actual pixels per line.
 *
 * if successed, file descriptor returned,
 * otherwise, -1 returned.
 */
int overlay2_open(char *dev, int  bpp, int  format, int  xpos, int  ypos, int  xres,
	int  yres, unsigned char  **map, int  *yoff, int  *ylen, int  *cboff, int  *cblen,
	int  *croff, int  *crlen, int  *pitch)
{
	struct fb_var_screeninfo var;
	struct fb_fix_screeninfo fix;
	int fd,err;

	if ( (!map) || (!yoff) || (!ylen) || (!cboff) || (!cblen) || (!croff) || (!crlen) )
		return -1;

	fd = open(dev, O_RDWR);
	if (fd < 0) return -1;

	var.xres = xres;
	var.yres = yres;
	var.bits_per_pixel = bpp;
	/* nonstd for FOR/XPOS/YPOS */
	var.nonstd = (format <<20) | (ypos << 10) | xpos;

	/* set "var" screeninfo */
	err = ioctl(fd, FBIOPUT_VSCREENINFO, &var);
	if (err) {
		close(fd);
		return -1;
	}

	/* get updated "fix" screeninfo */
    	err = ioctl(fd, FBIOGET_FSCREENINFO, &fix);
	if (err) {
		close(fd);
		return -1;
	}

	/* get updated "var" screeninfo */
	err = ioctl(fd, FBIOGET_VSCREENINFO, &var);
	if (err) {
		close(fd);
		return -1;
	}

	*map = (unsigned char*)mmap(0, fix.smem_len,
			PROT_READ | PROT_WRITE	, MAP_SHARED, fd, 0);

	if(*map == MAP_FAILED) {
		printf("mmap fb device fail!\n");
		close(fd);
		return -1;
	}

	*yoff   = var.red.offset;
	*ylen   = var.red.length;
	*cboff  = var.green.offset;
	*cblen  = var.green.length;
	*croff  = var.blue.offset;
	*crlen  = var.blue.length;

	*pitch = fix.line_length;
	
	printf("fbinfo:\nxres %d, yres %d, bpp %d\n",
			var.xres, var.yres, var.bits_per_pixel);
	printf("map   0x%08x, len   %d\n", (int)*map, fix.smem_len);
	printf("yoff  0x%08x, ylen  %d\n", *yoff,  *ylen);
	printf("cboff 0x%08x, cblen %d\n", *cboff, *cblen);
	printf("croff 0x%08x, crlen %d\n", *croff, *crlen);

	return fd;
}

/*
 * release overlay fd
 */
int overlay2_close(int fd)
{
	if (fd >= 0) close(fd);
	/* FIXME : we need mumap */
	return 0;
}

int main(int argc, char *argv[])
{
	int i, fd, fbfd;
	unsigned char *map;
	int ylen, yoff, cblen, cboff, crlen, croff, pitch;
	struct video_capability vc;
	struct video_window vw;
	struct video_picture vp;
	struct reg_set_s reg;
	struct video_mbuf vm;
	__u8 *buf;
	int mmap_ok=0, cnt;
	
	//fd = open("/dev/v4l/video0", O_RDONLY);
	fd = open("/dev/v4l/video0", O_RDWR);	//rd&wr
	if(fd<0) {
		fprintf(stderr, "Open camera fail!\n");
		return -1;
	} else
		fprintf(stdout, "Open camera success\n");

	if(camera_i2c_init()||sccb_init(fd)) {
		printf("fail to initialize the I2C interface of camera!\n");
		close(fd);
		return -1;
	}

	if(ioctl(fd, VIDIOCGCAP, &vc)<0)
		printf("VIDIOCGCAP fail\n");
	else
		printf("max width %d, height %d\nmin width %d, height %d\n",
			vc.maxwidth, vc.maxheight, vc.minwidth, vc.minheight);

	if (argc > 2) {
        vw.width = atoi(argv[1]);
        vw.height = atoi(argv[2]);
	} else {
		vw.width = 640;
		vw.height = 480;
	}
	if(ioctl(fd, VIDIOCSWIN, &vw)<0)
		printf("VIDIOCSWIN fail\n");

	if(ioctl(fd, VIDIOCGWIN, &vw)<0)
		printf("VIDIOCGWIN fail\n");
	else
		printf("current width %d, height %d\n",	vw.width, vw.height);

//	vp.palette = VIDEO_PALETTE_YUV422P;	// YUV 4:2:2 Planar
//	if(ioctl(fd, VIDIOCSPICT, &vp)<0)
//		printf("VIDIOCSPICT fail\n");

	if(ioctl(fd, VIDIOCGPICT, &vp)<0)
		printf("VIDIOCGPICT fail\n");
	else
		printf("current palette %d\n",	vp.palette);

	//extend ioctl
	reg.val1 = CAMERA_IMAGE_FORMAT_YCBCR422_PACKED;
	reg.val2 = CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR;
	if(ioctl(fd, WCAM_VIDIOCSINFOR, &reg)<0)
		printf("WCAM_VIDIOCSINFOR fail\n");

	if(ioctl(fd, WCAM_VIDIOCGINFOR, &reg)<0)
		printf("WCAM_VIDIOCGINFOR fail\n");
	else
		printf("current input format %d, output format %d\n",
			reg.val1, reg.val2);
	
	if(ioctl(fd, VIDIOCGMBUF, &vm)<0)
		printf("VIDIOCGMBUF fail\n");
	else
		printf("current camera buffer size %d, total frames %d\n",
			vm.size, vm.frames);
#ifdef MMAP_CAMERA
	puts("mmap camera buffer.\n");
	buf = (__u8 *)mmap(0, vm.size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
	if((int)buf==-1) {
		printf("fail to mmap camera device, use read method!\n");
		buf = malloc(vw.width*vw.height*2);
		if(!buf) {
			printf("fail to allocate memory to read camera!\n");
			close(fd);
			return -1;
		}
	} else
		mmap_ok = 1;
#else
	puts("allocate memory for camera buffer.\n");
	buf =  malloc(vw.width*vw.height*2);
	if(!buf) {
		printf("fail to allocate memory to read camera!\n");
		close(fd);
		return -1;
	}
#endif
	printf("buf %x\n", (int)buf);

	fbfd = overlay2_open("/dev/fb/2", 16, 0x3, 0, 0, vw.width, vw.height,
			&map, &yoff, &ylen, &cboff, &cblen, &croff, &crlen, &pitch);
	if ( fbfd < 0) {
		printf("Failed to open overlay2 device\n");
		if(mmap_ok)
			munmap(buf, vm.size);
		else
			free(buf);
		close(fd);
		exit(-1);
	}
	memset(buf, 0, vw.width*vw.height*2);
	memcpy(map+yoff,  buf, vw.width*vw.height);
	memcpy(map+cboff, buf+vw.width*vw.height, vw.width*vw.height/2);
	memcpy(map+croff, buf+vw.width*vw.height+vw.width*vw.height/2, vw.width*vw.height/2);

	test_ycbcr_to_rgb(map+yoff, map+cboff, map+croff, vw.width, vw.height);

	if(ioctl(fd, VIDIOCCAPTURE, STILL_IMAGE)<0)
//	if(ioctl(fd, VIDIOCCAPTURE, VIDEO_START)<0)
		printf("VIDIOCCAPTURE fail\n");

	cnt = 0;
	while(1) {
		//FILE *wfp;
		
		i = vw.width*vw.height*2;
		if(!mmap_ok) {
			i = read(fd, buf, i);
			if(i<0) {
				fprintf(stderr, "read fail! %d \n", i);
				break;
			}
			cnt++;
		}
	/*	fprintf(stdout, "read %d bytes\n", i);
		wfp = fopen("/tmp/dat", "w+b");
		if(wfp==NULL) {
			printf("fail to open file for save data!\n");
			break;
		}
		printf("write %d bytes.\n", fwrite(buf, i, 1, wfp)*i);
		fclose(wfp);
		break;
	*/
		memcpy(map+yoff,  buf, vw.width*vw.height);
		memcpy(map+cboff, buf+vw.width*vw.height, vw.width*vw.height/2);
		memcpy(map+croff, buf+vw.width*vw.height+vw.width*vw.height/2, vw.width*vw.height/2);
		//printf("Press enter to continue...\n");
		//getchar();
	}
	
	overlay2_close(fbfd);

	if(mmap_ok)
		munmap(buf, vm.size);
	else
		free(buf);
	close(fd);

	return 0;
}