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_INITstatic 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);	}	//printf("write fail %x %x\n", data[0], data[1]);	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);	}	//printf("read fail\n");	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, 0, id)) {		printf("fail to get camera chip version!\n");		close(fd);		return -1;	}	printf("chip version is 0x%02x\n", id[0]);	i2c_read(fd, 0x1f, id+1);	printf("decoder status is 0x%02x\n", id[1]);	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);	}//	if(cpu_type==1)	{	//s3c2440a//		i2c_write(fd, 0x15, 0x00);	//always has PCLK//		i2c_write(fd, 0x3a, 0x01);	//YCbYCr order//	}		close(fd);	return 0;}#else#define	camera_i2c_init()	0#endif#ifdef SCCB_INITstatic 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/*************************************************************/static void usage(void){	int i;	printf("testcamera [-x/y/p/m/v/o/f/u/h options]\n");	printf("-x width   (set output image width, must be 4 alignment)\n");	printf("-y height  (set output image height)\n");	printf("-p palette (set output image color space)\n");	printf("	optional image:\n");	for(i=0; i<MAX_IMAGE_FORMAT; i++)		printf("	[%2d] : %s\n", i, optional_image_format[i].name);	printf("-m         (use mmap method for camera)\n");	printf("-v         (capture video instead of still image)\n");	printf("-o number  (set save jpeg file optimization, must be 0~100)\n");	printf("-f name    (set save jpeg file prefix name)\n");	printf("-u arch    (set CPU type)\n");	printf("	current special CPU:\n");	printf("	pxa27x\n");	printf("	s3c2440a\n");	printf("	default is pxa27x\n");	printf("-h show help\n");}static int getoptions(int argc, char *argv[]){	int ret = 0;	char c;	opterr = 0;	//don't print error message	while ((c = getopt (argc, argv, "x:y:p:o:f:u:mvh")) != (char)EOF)	/* Scan the command line for options */	switch (c) {	case 'x':		image_width = atoi(optarg);		if((image_width%4)||(image_width>FIXED_SOURCE_WIDTH))			ret = -1;		break;	case 'y':		image_height = atoi(optarg);		if(image_height>FIXED_SOURCE_HEIGHT)			ret = -1;		break;	case 'p':		image_format = atoi(optarg);		if(image_format>=MAX_IMAGE_FORMAT)			ret = -1;		break;	case 'o':		optimization = atoi(optarg);		if(optimization>100)			ret = -1;		break;	case 'f':		image_file = optarg;		break;	case 'm':		mmap_camera = 1;		break;	case 'v':		capture_video = 1;		break;	case 'u':		if(strncmp(optarg, "pxa27x", 6)==0)			cpu_type = 0;		else if(strncmp(optarg, "s3c2440a", 8)==0)			cpu_type = 1;		else			printf("unknown CPU type\n");		break;	case 'h':		//ret = -1;		//break;	default:		ret = -1;		break;	}	return ret;}/*************************************************************/int main(int argc, char *argv[]){	int i, fd, fbfd;	struct fb_var_screeninfo vinfo;	struct fb_fix_screeninfo finfo;	__u8 *fb_buf;	__u32 screensize;	struct video_capability vc;	struct video_window vw;	struct video_capture vcp;	struct video_picture vp;	struct video_mbuf vm;	__u8 *buf = NULL;	int show, cnt;	fd_set rfds;	struct timeval tv;	struct termios tio;	int c_lflag;	fbfd = open("/dev/fb0", O_RDWR);	if (fbfd < 0) {		fbfd = open("/dev/fb/0", O_RDWR);		if(fbfd<0) {			printf("Error: cannot open framebuffer device.\n");		        return -1;		}	}	// Get fixed screen information	if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo)) {				printf("Error reading fixed information.\n");		close(fbfd);        	return -1;	}	// Get variable screen information	if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo)) {		printf("Error reading variable information.\n");		close(fbfd);		return -1;	}	printf("%dx%d, %dbpp\n", vinfo.xres, vinfo.yres, vinfo.bits_per_pixel );	fb_xres = vinfo.xres;	fb_yres = vinfo.yres;	fb_bpp  = vinfo.bits_per_pixel;	screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;	fb_buf = (char *)mmap(0, screensize, PROT_READ | PROT_WRITE, MAP_SHARED,				fbfd, 0);	if ((int)fb_buf == -1) {		printf("Error: failed to map framebuffer device to memory.\n");		close(fbfd);        	return -1;	}	test_ycbcr_to_rgb(fb_buf);	if(getoptions(argc, argv)<0) {		close(fbfd);		usage();		return -1;	}	fd = open("/dev/v4l/video0", O_RDONLY);	//rd&wr	if(fd<0) {		fprintf(stderr, "Open camera fail!\n");		close(fbfd);		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);		close(fbfd);		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);	vw.width  = FIXED_SOURCE_WIDTH;		//fixed in this application	vw.height = FIXED_SOURCE_HEIGHT;	//fixed in this application	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);	if(!image_width||!image_height) {		image_width  = vw.width;		image_height = vw.height;	}	//useful in s3c2440a	vcp.width  = image_width;	vcp.height = image_height;	if(ioctl(fd, VIDIOCSCAPTURE, &vcp)<0)		printf("VIDIOCSCAPTURE fail\n");	else {		if(ioctl(fd, VIDIOCGCAPTURE, &vcp)<0)			printf("VIDIOCGCAPTURE fail\n");		else			printf("capture width %d, height %d\n", vcp.width, vcp.height);	}	vp.palette = optional_image_format[image_format].palette;	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);			if(cpu_type==1) {		if(ioctl(fd, VIDIOCSYCbCr, 2)<0) {			printf("fail to set YCbCr order!\n");		}	}	if(cpu_type==0) {		struct reg_set_s reg;		//extend ioctl, just in pxa270		reg.val1 = CAMERA_IMAGE_FORMAT_YCBCR422_PACKED;	//input format for pxa270		reg.val2 = CAMERA_IMAGE_FORMAT_YCBCR422_PLANAR;	//output format for pxa270		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(mmap_camera) {		if(ioctl(fd, VIDIOCGMBUF, &vm)<0) {			printf("VIDIOCGMBUF fail\n");			mmap_camera = 0;		} else {			printf("current camera buffer size %d, total frames %d\n",				vm.size, vm.frames);					buf = (__u8 *)mmap(0, vm.size, PROT_READ, MAP_SHARED, fd, 0);			if((int)buf==-1) {				printf("mmap camera fail!\n");				mmap_camera = 0;			} else				puts("mmap camera ok.\n");		}	}	if(!mmap_camera) {		puts("allocate memory for camera buffer.\n");		buf = malloc(image_width*image_height*2);		if(!buf) {			printf("fail to allocate memory for camera!\n");			close(fd);			munmap(fb_buf, screensize);			close(fbfd);			return -1;		}	}	printf("buffer at 0x%08x\n", (int)buf);	//puts("press enter to start...\n");	//getchar();		if(tcgetattr(0, &tio)<0) {		printf("tcgetattr fail!\n");		goto fail;	} else {		c_lflag = tio.c_lflag;		tio.c_lflag &= ~(ICANON | ECHO);		if(tcsetattr(0, TCSAFLUSH, &tio)<0) {			printf("tcsetattr fail!\n");			goto fail;		}	}		printf("now start capture...\n");	printf("press Esc key to exit, 'c' to save picture, 'd' to enable/disable display\n");	if(capture_video) {		if(ioctl(fd, VIDIOCCAPTURE, VIDEO_START)<0) {			printf("VIDIOCCAPTURE fail\n");			goto fail;		}	} else if(!cpu_type){		if(ioctl(fd, VIDIOCCAPTURE, STILL_IMAGE)<0) {			printf("VIDIOCCAPTURE fail\n");			goto fail;		}	}		FD_ZERO(&rfds);	//FD_SET(0, &rfds);	//FD_SET(fd, &rfds);		show = 1;	cnt  = 0;	while(1) {		//in pxa270, if capture still image, can do it just one time		if(!capture_video&&cpu_type)			if(ioctl(fd, VIDIOCCAPTURE, STILL_IMAGE)<0) {				printf("VIDIOCCAPTURE fail\n");				break;			}		//FD_ZERO(&rfds);		FD_SET(0, &rfds);		FD_SET(fd, &rfds);		tv.tv_sec = 3;		tv.tv_usec = 0;		select(fd+1, &rfds, NULL,  NULL, &tv);		if(FD_ISSET(0, &rfds)) {			char cmd;			//printf("stdin input\n");			if(read(0, &cmd, 1)<0) {				printf("read stdin fail!\n");				break;			}			if(cmd=='d'||cmd=='D') {				show = !show;				printf("%s display\n", show?"enable":"disable");			} else if(cmd=='c'||cmd=='C') {				printf("save picture...\n");				save_picture(buf);			} else if(cmd==0x1b)				break;		} else if(FD_ISSET(fd, &rfds)) {			if(mmap_camera) {#if 1			//just for performance test in video capturing, int(rdy)->show->int->show			//use read to clear ready flag in driver				i = read(fd, buf, 0);	//mmap camera for read, so read 0 byte to buf				if(i<0) {					printf("read fail!%d\n", i);					break;				}#endif			} else {				i = read(fd, buf, image_width*image_height*2);				if(i<0) {					fprintf(stderr, "read fail! %d\n", i);					break;				}				//printf("read %d bytes\n", i);				cnt++;			}		} else {			printf("wait timeout, break...\n");			break;		}#if 0		{			FILE *wfp;			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;		}#endif		if(show)			show_cam_img(fb_buf, buf,					buf+image_width*image_height,					buf+(image_width*image_height*3)/2);	}	tio.c_lflag = c_lflag;	tcsetattr(0, TCSAFLUSH, &tio);fail:	if(mmap_camera)		munmap(buf, vm.size);	else		free(buf);	close(fd);	munmap(fb_buf, screensize);	close(fbfd);	return 0;}