Initial qrdecode4display.

Signed-off-by: Yi Sun <yi.sun@intel.com>

4 files changed, 1360 insertions, 1 deletions

diff --git a/include/decodeqr.h b/include/decodeqr.h
new file mode 100644
index 0000000..86d8c01
--- /dev/null
+++ b/include/decodeqr.h
@@ -0,0 +1,286 @@
+/////////////////////////////////////////////////////////////////////////
+//
+// libdecodeqr.h --a part of libdecodeqr
+//
+// Copyright(C) 2007 NISHI Takao <zophos@koka-in.org>
+//                   JMA  (Japan Medical Association)
+//                   NaCl (Network Applied Communication Laboratory Ltd.)
+//
+// This is free software with ABSOLUTELY NO WARRANTY.
+// You can redistribute and/or modify it under the terms of LGPL.
+//
+// $Id$
+//
+#ifndef __QR_DECODER__
+#define __QR_DECODER__
+
+#include <cv.h>
+#include "qrerror.h"
+#include "qrtypes.h"
+
+#define DEFAULT_ADAPTIVE_TH_SIZE 25
+#define DEFAULT_ADAPTIVE_TH_DELTA 10
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/////////////////////////////////////////////////////////////////////////
+//
+// initializer
+//
+// ARGS: none
+// RETURN:
+//   QrDecoderHandle handle
+//
+extern QrDecoderHandle qr_decoder_open();
+
+/////////////////////////////////////////////////////////////////////////
+//
+// initializer with source image size
+//
+// ARGS:
+//   int width:  pixel width of source image
+//   int height: pixel height of source image
+//   int depth:  image depth (bit par pixel; use OpenCV IPL_DEPTH_*)
+//   int channel: number of image channel
+//
+// RETURN:
+//   QrDecoderHandle handle
+//
+// NOTE:
+//   24-bit full color image has IPL_DEPTH_8U depth and 3 channels.
+//
+extern QrDecoderHandle qr_decoder_open_with_image_size(
+    int width,int height,int depth,int channel);
+
+
+/////////////////////////////////////////////////////////////////////////
+//
+// finalizer
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//
+// RETURN: none
+//
+extern void qr_decoder_close(QrDecoderHandle decoder);
+
+
+/////////////////////////////////////////////////////////////////////////
+//
+// get status
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//
+// RETURN: status code
+//
+extern short qr_decoder_get_status(QrDecoderHandle decoder);
+
+/////////////////////////////////////////////////////////////////////////
+//
+// get working status
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//
+// RETURN: status code
+//
+extern int qr_decoder_is_busy(QrDecoderHandle decoder);
+
+
+/////////////////////////////////////////////////////////////////////////
+//
+// set source image size
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//   int width:  pixel width of source image
+//   int height: pixel height of source image
+//   int depth:  image depth (bit par pixel; use OpenCV IPL_DEPTH_*)
+//   int channel: number of image channel
+//
+// RETURN:
+//   QrDecoderHandle handle
+//
+// NOTE:
+//   This method provide same function as qr_decoder_open_with_image_size().
+//
+extern QrDecoderHandle qr_decoder_set_image_size(
+    QrDecoderHandle decoder,int width,int height,int depth,int channel);
+
+
+/////////////////////////////////////////////////////////////////////////
+//
+// preset gaven image as source image
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//   IplImage *src: source image
+//
+// RETURN:
+//   QrDecoderHandle handle
+//
+extern QrDecoderHandle qr_decoder_set_image_buffer(
+    QrDecoderHandle decoder,IplImage *src);
+
+/////////////////////////////////////////////////////////////////////////
+//
+// get source image buffer
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//
+// RETURN:
+//   IplImage *: pointer to buffer source image|NULL
+//
+// NOTE:
+//   See OpenCV reference manual to access to IplImage *
+//
+extern IplImage *qr_decoder_get_image_buffer(QrDecoderHandle decoder);
+
+extern IplImage *qr_decoder_get_transformed_image_buffer(
+    QrDecoderHandle decoder);
+extern IplImage *qr_decoder_get_binarized_image_buffer(
+    QrDecoderHandle decoder);
+extern IplImage *qr_decoder_get_tmp_image_buffer(
+    QrDecoderHandle decoder);
+
+/////////////////////////////////////////////////////////////////////////
+//
+// decode preset source image
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//   int adaptive_th_size: value of AdaptiveThreshold size
+//   int adaptive_th_delta: value of AdaptiveThreshold delta
+//
+// RETURN:
+//   short: status code of decoder
+//
+// NOTE:
+//   On succeeded, status code has 0x2000.
+//   See qrtypes.h for details of status code.
+//
+//   In case of adaptive_th_size=0, binarizing methods will be
+//   used cvThreshlod() instead of cvAdaptiveThreshold()
+//
+#ifdef __cplusplus
+extern short qr_decoder_decode(QrDecoderHandle decoder,
+                               int adaptive_th_size=
+                               DEFAULT_ADAPTIVE_TH_SIZE,
+                               int adaptive_th_delta=
+                               DEFAULT_ADAPTIVE_TH_DELTA);
+#else
+extern short qr_decoder_decode(QrDecoderHandle decoder,
+                               int adaptive_th_size,
+                               int adaptive_th_delta);
+#endif
+
+/////////////////////////////////////////////////////////////////////////
+//
+// decode gaven image
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//   IplImage *src: image to decode
+//   int adaptive_th_size: value of AdaptiveThreshold size
+//   int adaptive_th_delta: value of AdaptiveThreshold delta
+//
+// RETURN:
+//   short: status code of decoder
+//
+#ifdef __cplusplus
+extern short qr_decoder_decode_image(QrDecoderHandle decoder,
+                                     IplImage *src,
+                                     int adaptive_th_size=
+                                     DEFAULT_ADAPTIVE_TH_SIZE,
+                                     int adaptive_th_delta=
+                                     DEFAULT_ADAPTIVE_TH_DELTA);
+#else
+extern short qr_decoder_decode_image(QrDecoderHandle decoder,
+                                     IplImage *src,
+                                     int adaptive_th_size,
+                                     int adaptive_th_delta);
+#endif
+
+/////////////////////////////////////////////////////////////////////////
+//
+// get abstruction of decoded data 
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//   QrCodeHeader *header: pointer to buffer of header
+//
+// RETURN:
+//   1 (on success)||0 (on error)
+//
+extern int qr_decoder_get_header(QrDecoderHandle decoder,
+                                 QrCodeHeader *header);
+
+/////////////////////////////////////////////////////////////////////////
+//
+// get decoded text data 
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//   unsigned char *buf: pointer to buffer of header
+//   int buf_size: buffer size
+//
+// RETURN:
+//   copied data size||0 (on error)
+//
+// NOTE:
+//   The data DOES NOT TERMINATE with null.
+//   To get actual buffer size, use QrCodeHeader's .byte_size element.
+//
+extern int qr_decoder_get_body(QrDecoderHandle decoder,
+                               unsigned char *buf,int buf_size);
+
+
+/////////////////////////////////////////////////////////////////////////
+//
+// get vertexes of decoded code region
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//
+// RETURN:
+//   Pointer to CvPoint[4] which consist vertexes of code region
+//
+extern CvPoint *qr_decoder_get_coderegion_vertexes(QrDecoderHandle decoder);
+
+
+/////////////////////////////////////////////////////////////////////////
+//
+// get Box array of decoded finder patterns
+//
+// ARGS:
+//   QrDecoderHandle decoder: handler
+//
+// RETURN:
+//   Pointer to CvBox2D[3] which consist boxes of finder pattern
+//
+extern CvBox2D *qr_decoder_get_finderpattern_boxes(QrDecoderHandle decoder);
+
+
+/////////////////////////////////////////////////////////////////////////
+//
+// version information
+//
+extern char *qr_decoder_version();
+extern char *qr_decoder_version_description();
+extern char *qr_decoder_version_product();
+extern int qr_decoder_version_major();
+extern int qr_decoder_version_minor();
+extern int qr_decoder_version_teeny();
+extern char *qr_decoder_version_suffix();
+extern char *qr_decoder_version_revision();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/lib/libdecodeqr.so b/lib/libdecodeqr.so
new file mode 100755
index 0000000..e7fd908
--- /dev/null
+++ b/lib/libdecodeqr.so
diff --git a/tests/Makefile.sources b/tests/Makefile.sources
index 623a748..6ab7bcf 100644
--- a/tests/Makefile.sources
+++ b/tests/Makefile.sources
@@ -13,4 +13,7 @@ testdisplay_SOURCES = \
 	testdisplay_hotplug.c \
 	$(NULL)
 
-TESTS_progs += testdisplay
+QRdecode4Display_SOURCES = QRdecode4Display.c \
+	${NULL}
+
+TESTS_progs += testdisplay QRdecode4Display
diff --git a/tests/QRdecode4Display.c b/tests/QRdecode4Display.c
new file mode 100644
index 0000000..8324b17
--- /dev/null
+++ b/tests/QRdecode4Display.c
@@ -0,0 +1,1070 @@
+#include <stdio.h>
+#include <highgui.h>
+#include <decodeqr.h>
+#include <opencv2/highgui/highgui.hpp>
+
+/*
+ *	V4L2 video capture example
+ *
+ *	This program can be used and distributed without restrictions.
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#include <getopt.h>				/* getopt_long() */
+
+#include <fcntl.h>				/* low-level i/o */
+#include <unistd.h>
+#include <errno.h>
+#include <malloc.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+#include <asm/types.h>			/* for videodev2.h */
+
+#include <linux/videodev2.h>
+
+#include <linux/fb.h>			/*for framebuffer*/
+
+#include <signal.h>				/*handle signal event*/
+
+#include "xf86drm.h"			/*used in dpms on*/
+#include "xf86drmMode.h"
+
+#include <dirent.h>
+
+#define CLEAR(x) memset (&(x), 0, sizeof (x))
+
+#define CAMWIDTH 1280
+#define CAMHEIGHT 720
+
+#define MAX_DEV 4
+#define MAX_FILE_LEN 128
+#define MAX_CAP_FRAME 15
+
+typedef enum {
+	IO_METHOD_READ,
+	IO_METHOD_MMAP,
+	IO_METHOD_USERPTR,
+} io_method;
+
+struct buffer {
+	void * start;
+	size_t length;
+};
+
+static io_method io = IO_METHOD_MMAP;
+
+//const static char *dev_name = "/dev/video0";
+static char dev_names[MAX_DEV][MAX_FILE_LEN] = { };
+static int dev_count = 0;
+
+//static int fd = -1;
+static int dev_fds[MAX_DEV] = { -1 };
+//struct buffer *buffers = NULL;
+struct buffer *dev_buffers[MAX_DEV] = { NULL };
+//static unsigned int n_buffers = 0;
+static unsigned int dev_buffers_count[MAX_DEV] = { 0 };
+
+static int pre_view = 0;
+static int run_test = 0;
+static int pid_test = 0;
+static char testdisplaydir[256];
+static char QR4Ddir[256];
+static char child_exist = 0;
+
+char *screen_ori;
+
+void sighandler(int signo)
+{
+	printf("Exit:The sing is:%d\n");
+	exit(signo);
+}
+
+void child_handler(int num)   
+{	
+	child_exist = 0;
+	return;
+}  
+
+__attribute__ ((constructor))
+void save_screen()
+{
+	int fbfd = 0;
+	struct fb_var_screeninfo vinfo;
+	struct fb_fix_screeninfo finfo;
+	long int screensize = 0;
+	char *fbp = 0;
+
+	// Open the file for reading and writing
+	fbfd = open("/dev/fb0", O_RDWR);
+	if (fbfd == -1) {
+		perror("Error: cannot open framebuffer device");
+		exit(1);
+	}
+	printf("The framebuffer device was opened successfully.\n");
+
+	// Get fixed screen information
+	if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo) == -1) {
+		perror("Error reading fixed information");
+		exit(2);
+	}
+
+	// Get variable screen information
+	if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo) == -1) {
+		perror("Error reading variable information");
+		exit(3);
+	}
+
+	// Figure out the size of the screen in bytes
+	screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
+	screen_ori =(char*) malloc(screensize);
+
+	// Map the device to memory
+	fbp = (char *)mmap(0, screensize, PROT_READ | PROT_WRITE, MAP_SHARED, fbfd, 0);
+	if ( fbp == MAP_FAILED ) {
+		perror("Error: failed to map framebuffer device to memory");
+		exit(4);
+	}
+
+	memcpy(screen_ori, fbp, screensize );
+	munmap(fbp, screensize);
+	close(fbfd);
+
+	int sigs[] = {
+		SIGILL, SIGFPE, SIGABRT, SIGBUS,
+		SIGSEGV, SIGHUP, SIGINT, SIGQUIT,
+		SIGTERM
+	};
+
+	struct sigaction sa;
+	sa.sa_handler = sighandler;
+	sigemptyset(&sa.sa_mask);
+	sa.sa_flags = SA_RESETHAND;
+
+	for(int i = 0; i < sizeof(sigs)/sizeof(sigs[0]); ++i) {
+		if (sigaction(sigs[i], &sa, NULL) == -1) {
+			perror("Could not set signal handler");
+		}
+	}
+
+	sa.sa_handler = child_handler;
+	sigemptyset(&sa.sa_mask);
+	sa.sa_flags = SA_RESETHAND;
+	sigaction( SIGCHLD, &sa, NULL);
+
+//	signal(SIGCHLD,child_handler);
+}
+
+__attribute__ ((destructor))
+void resume_screen()
+{
+	int fbfd = 0;
+	struct fb_var_screeninfo vinfo;
+	struct fb_fix_screeninfo finfo;
+	long int screensize = 0;
+	char *fbp = 0;
+
+	// Open the file for reading and writing
+	fbfd = open("/dev/fb0", O_RDWR);
+	if (fbfd == -1) {
+		perror("Error: cannot open framebuffer device");
+		exit(1);
+	}
+
+	// Get fixed screen information
+	if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo) == -1) {
+		perror("Error reading fixed information");
+		exit(2);
+	}
+
+	// Get variable screen information
+	if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo) == -1) {
+		perror("Error reading variable information");
+		exit(3);
+	}
+
+	// Figure out the size of the screen in bytes
+	screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
+
+	// Map the device to memory
+	fbp = (char *)mmap(0, screensize, PROT_READ | PROT_WRITE, MAP_SHARED, fbfd, 0);
+	if ( fbp == MAP_FAILED ) {
+		perror("Error: failed to map framebuffer device to memory");
+		exit(4);
+	}
+
+	memcpy(fbp, screen_ori, screensize );
+
+	free(screen_ori);
+	munmap(fbp, screensize);
+	close(fbfd);
+
+	kill(pid_test, SIGUSR2);
+	return;
+}
+
+
+int yuv2rgb_pixel(int y, int u, int v)
+{
+	unsigned int pixel32 = 0;
+	unsigned char *pixel = (unsigned char *)&pixel32;
+	int r, g, b;
+	r = y + (1.370705 * (v-128));
+	g = y - (0.698001 * (v-128)) - (0.337633 * (u-128));
+	b = y + (1.732446 * (u-128));
+	if(r > 255) r = 255;
+	if(g > 255) g = 255;
+	if(b > 255) b = 255;
+	if(r < 0) r = 0;
+	if(g < 0) g = 0;
+	if(b < 0) b = 0;
+	pixel[0] = b * 220 / 256;
+	pixel[1] = g * 220 / 256;
+	pixel[2] = r * 220 / 256;
+	return pixel32;
+}
+
+int yuv2rgb(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height)
+{
+	unsigned int in, out = 0;
+	unsigned int pixel_16;
+	unsigned char pixel_24[3];
+	unsigned int pixel32;
+	int y0, u, y1, v;
+
+	for(in = 0; in < width * height * 2; in += 4) {
+		pixel_16 = yuv[in + 3] << 24 | yuv[in + 2] << 16 | yuv[in + 1] <<  8 | yuv[in + 0];
+
+		y0 = (pixel_16 & 0x000000ff);
+		u  = (pixel_16 & 0x0000ff00) >>  8;
+		y1 = (pixel_16 & 0x00ff0000) >> 16;
+		v  = (pixel_16 & 0xff000000) >> 24;
+
+		pixel32 = yuv2rgb_pixel(y0, u, v);
+
+		pixel_24[0] = (pixel32 & 0x000000ff);
+		pixel_24[1] = (pixel32 & 0x0000ff00) >> 8;
+		pixel_24[2] = (pixel32 & 0x00ff0000) >> 16;
+		rgb[out++] = pixel_24[0];
+		rgb[out++] = pixel_24[1];
+		rgb[out++] = pixel_24[2];
+
+		pixel32 = yuv2rgb_pixel(y1, u, v);
+
+		pixel_24[0] = (pixel32 & 0x000000ff);
+		pixel_24[1] = (pixel32 & 0x0000ff00) >> 8;
+		pixel_24[2] = (pixel32 & 0x00ff0000) >> 16;
+		rgb[out++] = pixel_24[0];
+		rgb[out++] = pixel_24[1];
+		rgb[out++] = pixel_24[2];
+	}
+	return 0;
+}
+
+static void
+errno_exit (const char *s)
+{
+	fprintf (stderr, "%s error %d, %s\n", s, errno, strerror (errno));
+	exit (EXIT_FAILURE);
+}
+
+static int
+xioctl(int fd, int request, void *arg)
+{
+	int r;
+
+	do r = ioctl (fd, request, arg);
+	while (-1 == r && EINTR == errno);
+
+	return r;
+}
+
+void paint_screen( IplImage *image )
+{
+	int fbfd = 0;
+	struct fb_var_screeninfo vinfo;
+	struct fb_fix_screeninfo finfo;
+	long int screensize = 0;
+	char *fbp = 0;
+	int x = 0, y = 0;
+	long int location = 0;
+
+	// Open the file for reading and writing
+	fbfd = open("/dev/fb0", O_RDWR);
+	if (fbfd == -1) 
+		errno_exit("cannot open framebuffer device");
+
+	// Get fixed screen information
+	if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo) == -1) 
+		errno_exit("reading fixed information");
+	
+
+	// Get variable screen information
+	if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo) == -1) 
+		errno_exit("reading variable information");
+
+	// Figure out the size of the screen in bytes
+	screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;
+
+	// Map the device to memory
+	fbp = (char *)mmap(0, screensize, PROT_READ | PROT_WRITE, MAP_SHARED, fbfd, 0);
+	if ( fbp == MAP_FAILED ) 
+		errno_exit("failed to map framebuffer device to memory");
+
+	for (y = 0; y < CAMHEIGHT; y++)
+	{
+		for (x = 0; x < CAMWIDTH; x++) 
+		{
+			location = (x+vinfo.xoffset) * (vinfo.bits_per_pixel/8) +
+					   (y+vinfo.yoffset) * finfo.line_length;
+
+			if (vinfo.bits_per_pixel == 32) {
+				*(fbp + location) =  image->imageData[ y*CAMWIDTH*3 + x*3 + 0];		// Some blue
+				*(fbp + location + 1) = image->imageData[ y*CAMWIDTH*3 + x*3 + 1];	// A little green
+				*(fbp + location + 2) = image->imageData[ y*CAMWIDTH*3 + x*3 + 2];	  // A lot of red
+				*(fbp + location + 3) = 0;		// No transparency
+			} else	{ //assume 16bpp
+				int b = 10;
+				int g = (x-100)/6;	   // A little green
+				int r = 31-(y-100)/16;	  // A lot of red
+				unsigned short int t = r<<11 | g << 5 | b;
+				*((unsigned short int*)(fbp + location)) = t;
+			}
+		}
+	}
+	munmap(fbp, screensize);
+	close(fbfd);
+}
+
+static void exist_dir(char *dir)
+{
+	if(access(dir,0) == -1)
+		if (mkdir( dir,0777))//create it if not exist
+			errno_exit("create folder failed");
+}
+
+static void
+process_image(void *p, int dev_id)
+{
+	static int f_count = 0;
+	static int mode_count = 0;
+	char *QR_code = NULL;
+
+	const char waiting[] = {'-', '\\', '|', '/'};
+	char filename[256];
+	memset(filename, 0, 256);
+
+	f_count++;
+
+	IplImage *image = cvCreateImageHeader(cvSize(CAMWIDTH, CAMHEIGHT),IPL_DEPTH_8U,3);;  
+
+	//YUYV to RGB the space of the image enlarge.
+	unsigned char *rgbimg =(unsigned char*) malloc(dev_buffers[dev_id][0].length * 3 / 2); 
+
+	yuv2rgb((unsigned char*)p, rgbimg, CAMWIDTH, CAMHEIGHT);	
+	cvSetData(image,rgbimg,CAMWIDTH*3);  
+
+	// initialize
+	QrDecoderHandle decoder = qr_decoder_open();
+
+	// do decode using default parameter
+	qr_decoder_decode_image(decoder, image);
+
+	// get QR code header
+	QrCodeHeader header;
+	if(qr_decoder_get_header(decoder,&header)){
+
+		// get QR code text
+		// To null terminate, a buffer size is larger than body size.
+		QR_code = new char[header.byte_size+1];
+
+		qr_decoder_get_body(decoder,(unsigned char *)QR_code ,header.byte_size+1);
+		printf("QR string: %s\n:", QR_code );
+
+	}
+
+	qr_decoder_close(decoder);
+
+	printf("\r%c%d\r", waiting[f_count%4], f_count);
+	fflush (stdout);
+
+	if( pre_view )
+	{
+		paint_screen(image);
+		return;
+	}
+
+	if( QR_code && strstr(QR_code, "pass") )
+	{
+		f_count = 0;
+
+		sprintf(filename, "%s/saveimages/",QR4Ddir);
+		exist_dir(filename);
+		chdir(filename);
+
+		memset(filename, 0, 256);
+
+		sprintf(filename, "pass_%d_%d.png",mode_count, dev_id);
+		cvSaveImage(filename, image);
+		printf("SaveAs:%s\n", filename);
+
+		kill(pid_test, SIGUSR1);
+		mode_count ++;
+	}
+	else if ( f_count/dev_count == MAX_CAP_FRAME)
+	{
+		sprintf(filename, "%s/saveimages/",QR4Ddir);
+		exist_dir(filename);
+		chdir(filename);
+
+		memset(filename, 0, 256);
+
+		sprintf(filename, "fail_%d_%d.png",mode_count, dev_id);
+		cvSaveImage(filename, image);
+		printf("SaveAs:%s\n", filename);
+	}
+	else if ( f_count/dev_count > MAX_CAP_FRAME )
+	{
+		f_count = 0;
+
+		kill(pid_test, SIGUSR1);
+		mode_count ++;
+	}
+		
+	// finalize
+	delete[] QR_code;
+}
+
+static int
+read_frame(int dev_id)
+{
+	struct v4l2_buffer buf;
+	unsigned int i;
+
+	switch (io) {
+	case IO_METHOD_READ:
+		if (-1 == read (dev_fds[dev_id], dev_buffers[dev_id][0].start, dev_buffers[dev_id][0].length)) {
+			switch (errno) {
+				case EAGAIN:
+					return 0;
+
+				case EIO:
+				/* Could ignore EIO, see spec. */
+				/* fall through */
+
+				default:
+					errno_exit ("read");
+			}
+		}
+
+		process_image (dev_buffers[dev_id][0].start, dev_id);
+
+		break;
+
+	case IO_METHOD_MMAP:
+		CLEAR (buf);
+
+		buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		buf.memory = V4L2_MEMORY_MMAP;
+
+		if (-1 == xioctl (dev_fds[dev_id], VIDIOC_DQBUF, &buf)) {
+			switch (errno) {
+				case EAGAIN:
+				return 0;
+
+			case EIO:
+			/* Could ignore EIO, see spec. */
+			/* fall through */
+
+			default:
+				errno_exit ("VIDIOC_DQBUF");
+			}
+		}
+
+		assert (buf.index < dev_buffers_count[dev_id]);
+
+		process_image (dev_buffers[dev_id][buf.index].start, dev_id);
+
+		if (-1 == xioctl (dev_fds[dev_id], VIDIOC_QBUF, &buf))
+			errno_exit ("VIDIOC_QBUF");
+
+		break;
+	}
+
+	return 1;
+}
+
+static void
+stop_capturing(int dev_id)
+{
+	enum v4l2_buf_type type;
+
+	switch (io) {
+	case IO_METHOD_READ:
+		/* Nothing to do. */
+		break;
+
+	case IO_METHOD_MMAP:
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+		if (-1 == xioctl (dev_fds[dev_id], VIDIOC_STREAMOFF, &type))
+			errno_exit ("VIDIOC_STREAMOFF");
+
+		break;
+	}
+}
+
+static void
+start_capturing(int dev_id)
+{
+	unsigned int i;
+	enum v4l2_buf_type type;
+
+	switch (io) {
+	case IO_METHOD_READ:
+		/* Nothing to do. */
+		break;
+
+	case IO_METHOD_MMAP:
+		for (i = 0; i < dev_buffers_count[dev_id]; ++i) {
+			struct v4l2_buffer buf;
+
+			CLEAR (buf);
+
+			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+			buf.memory = V4L2_MEMORY_MMAP;
+			buf.index = i;
+
+			if (-1 == xioctl (dev_fds[dev_id], VIDIOC_QBUF, &buf))
+				errno_exit ("VIDIOC_QBUF");
+		}
+
+		type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+		if (-1 == xioctl (dev_fds[dev_id], VIDIOC_STREAMON, &type))
+			errno_exit ("VIDIOC_STREAMON");
+
+		break;
+	}
+}
+
+static void
+mainloop(void)
+{
+	int count;
+
+	if (pre_view)
+		count = 108000;
+	else
+		count = 0;
+
+	while (count-- > 0 || child_exist) {
+		for (int k=0; k < dev_count; k++) {
+
+			start_capturing( k );
+
+			fd_set fds;
+			struct timeval tv;
+			int r;
+
+			FD_ZERO (&fds);
+			FD_SET (dev_fds[k], &fds);
+
+			/* Timeout. */
+			tv.tv_sec = 2;
+			tv.tv_usec = 0;
+
+			r = select (dev_fds[k] + 1, &fds, NULL, NULL, &tv);
+
+			if (-1 == r) {
+				if (EINTR == errno)
+				{
+					stop_capturing( k );
+					continue;
+				}
+				errno_exit ("select");
+			}
+
+
+			if (0 == r) {
+				fprintf (stderr, "select timeout\n");
+				exit (EXIT_FAILURE);
+			}
+
+			read_frame(k);
+
+			stop_capturing( k );
+		}
+	}
+}
+
+static void
+uninit_device(int dev_id)
+{
+	unsigned int i;
+
+	switch (io) {
+	case IO_METHOD_READ:
+		free (dev_buffers[dev_id][0].start);
+		break;
+
+	case IO_METHOD_MMAP:
+		for (i = 0; i < dev_buffers_count[dev_id]; ++i)
+			if (-1 == munmap (dev_buffers[dev_id][i].start, dev_buffers[dev_id][i].length))
+				errno_exit ("munmap");
+		break;
+	}
+
+	free (dev_buffers[dev_id]);
+}
+
+static void
+init_read(unsigned int buffer_size, int dev_id)
+{
+	dev_buffers[dev_id] = (struct buffer*)calloc (1, sizeof (*dev_buffers[dev_id]));
+
+	if (!dev_buffers[dev_id]) {
+		fprintf (stderr, "Out of memory\n");
+		exit (EXIT_FAILURE);
+	}
+
+	dev_buffers[dev_id][0].length = buffer_size;
+	dev_buffers[dev_id][0].start = malloc (buffer_size);
+
+	if (!dev_buffers[dev_id][0].start) {
+		fprintf (stderr, "Out of memory\n");
+		exit (EXIT_FAILURE);
+	}
+}
+
+static void
+init_mmap (int dev_id)
+{
+	struct v4l2_requestbuffers req;
+
+	CLEAR (req);
+
+	req.count = 2;
+	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	req.memory = V4L2_MEMORY_MMAP;
+
+	if (-1 == xioctl (dev_fds[dev_id], VIDIOC_REQBUFS, &req)) {
+		if (EINVAL == errno) {
+			fprintf (stderr, "%s does not support memory mapping\n", dev_names[dev_id]);
+			exit (EXIT_FAILURE);
+		} else {
+			errno_exit ("VIDIOC_REQBUFS");
+		}
+	}
+
+	if (req.count < 2) {
+		fprintf (stderr, "Insufficient buffer memory on %s\n", dev_names[dev_id]);
+		exit (EXIT_FAILURE);
+	}
+
+	//dev_buffers[dev_id] = (struct buffer*)calloc (req.count, sizeof (*dev_buffers[dev_id]));
+	dev_buffers[dev_id] = (struct buffer*)calloc (req.count, sizeof (struct buffer));
+
+	if (!dev_buffers[dev_id]) 
+		errno_exit("Out of memory\n");
+
+	for (dev_buffers_count[dev_id] = 0; dev_buffers_count[dev_id] < req.count; ++dev_buffers_count[dev_id]) {
+		struct v4l2_buffer buf;
+
+		CLEAR (buf);
+
+		buf.type		= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		buf.memory		= V4L2_MEMORY_MMAP;
+		buf.index		= dev_buffers_count[dev_id];
+
+		if (-1 == xioctl (dev_fds[dev_id], VIDIOC_QUERYBUF, &buf))
+			errno_exit ("VIDIOC_QUERYBUF");
+
+		dev_buffers[dev_id][dev_buffers_count[dev_id]].length = buf.length;
+		dev_buffers[dev_id][dev_buffers_count[dev_id]].start = mmap (NULL /* start anywhere */,
+				buf.length,
+				PROT_READ | PROT_WRITE /* required */,
+				MAP_SHARED /* recommended */,
+				dev_fds[dev_id], buf.m.offset);
+
+		if (MAP_FAILED == dev_buffers[dev_id][ dev_buffers_count[dev_id] ].start)
+			errno_exit ("mmap");
+	}
+}
+
+static void
+init_device( int dev_id )
+{
+	struct v4l2_capability cap;
+	struct v4l2_cropcap cropcap;
+	struct v4l2_crop crop;
+	struct v4l2_format fmt;
+	unsigned int min;
+
+	if (-1 == xioctl (dev_fds[dev_id], VIDIOC_QUERYCAP, &cap)) {
+		if (EINVAL == errno) {
+			fprintf (stderr, "%s is no V4L2 device\n", dev_names[dev_id]);
+			exit (EXIT_FAILURE);
+		} else {
+			errno_exit ("VIDIOC_QUERYCAP");
+		}
+	}
+
+	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+		fprintf (stderr, "%s is no video capture device\n", dev_names[dev_id]);
+		exit (EXIT_FAILURE);
+	}
+
+	switch (io) {
+	case IO_METHOD_READ:
+		if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
+			fprintf (stderr, "%s does not support read i/o\n", dev_names[dev_id]);
+			exit (EXIT_FAILURE);
+		}
+
+		break;
+
+	case IO_METHOD_MMAP:
+		if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+			fprintf (stderr, "%s does not support streaming i/o\n", dev_names[dev_id]);
+			exit (EXIT_FAILURE);
+		}
+
+		break;
+	}
+
+		/* Select video input, video standard and tune here. */
+
+
+	CLEAR (cropcap);
+
+	cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	if (0 == xioctl (dev_fds[dev_id], VIDIOC_CROPCAP, &cropcap)) {
+		crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		crop.c = cropcap.defrect; /* reset to default */
+
+		if (-1 == xioctl (dev_fds[dev_id], VIDIOC_S_CROP, &crop)) {
+				switch (errno) {
+					case EINVAL:
+						/* Cropping not supported. */
+						break;
+					default:
+						/* Errors ignored. */
+						break;
+				}
+		}
+	} else {	
+			/* Errors ignored. */
+	}
+
+
+	CLEAR (fmt);
+
+	fmt.type			= V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	fmt.fmt.pix.width		= CAMWIDTH; 
+	fmt.fmt.pix.height		= CAMHEIGHT;
+	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+	//fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_BGR24;
+	fmt.fmt.pix.field		= V4L2_FIELD_INTERLACED;
+
+	if (-1 == xioctl (dev_fds[dev_id], VIDIOC_S_FMT, &fmt))
+			errno_exit ("VIDIOC_S_FMT");
+
+	/* Note VIDIOC_S_FMT may change width and height. */
+
+	/* Buggy driver paranoia. */
+	min = fmt.fmt.pix.width * 2;
+	if (fmt.fmt.pix.bytesperline < min)
+		fmt.fmt.pix.bytesperline = min;
+	min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+	if (fmt.fmt.pix.sizeimage < min)
+		fmt.fmt.pix.sizeimage = min;
+
+	printf("pix:width:%d; height:%d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
+	switch (io) {
+	case IO_METHOD_READ:
+		init_read (fmt.fmt.pix.sizeimage, dev_id);
+		break;
+
+	case IO_METHOD_MMAP:
+		init_mmap ( dev_id);
+		break;
+	}
+}
+
+static void
+close_device(int dev_id)
+{
+	if (-1 == close (dev_fds[dev_id]))
+		errno_exit ("close");
+
+	dev_fds[dev_id] = -1;
+}
+
+static void get_devices(void)
+{
+	DIR* dir_info; //Directory Point
+	struct dirent* dir_entry; //Directory Entry Point
+	
+	dir_info = opendir("/dev/");
+	if( dir_info ){
+		while ( (dir_entry = readdir(dir_info)) != NULL)
+		{
+	             //Ignore the two special directory
+				if( strcmp(dir_entry->d_name, "..") == 0 
+						|| strcmp(dir_entry->d_name, ".") == 0 )
+					continue;
+
+				if ( strstr(dir_entry->d_name, "video") > 0)
+				{
+					sprintf( dev_names[dev_count], "/dev/%s",
+							dir_entry->d_name);
+					dev_count++;
+				}
+		}
+		closedir(dir_info);
+	}
+
+	for(int i=0; i<dev_count; i++)
+	{
+		printf("%s\n", dev_names[i]);
+	}
+}
+
+static int
+open_device	(char* dev_name)
+{
+	struct stat st;
+	int fd;
+
+	if (-1 == stat (dev_name, &st)) {
+		fprintf (stderr, "Cannot identify '%s': %d, %s\n",
+			 dev_name, errno, strerror (errno));
+		exit (EXIT_FAILURE);
+	}
+
+	if (!S_ISCHR (st.st_mode)) {
+		fprintf (stderr, "%s is no device\n", dev_name);
+		exit (EXIT_FAILURE);
+	}
+
+	fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
+
+	if (-1 == fd) {
+		fprintf (stderr, "Cannot open '%s': %d, %s\n",
+			 dev_name, errno, strerror (errno));
+		exit (EXIT_FAILURE);
+	}
+
+	return fd;
+}
+
+static void
+dpms_on()
+{
+    int drm_fd;
+    int conn_id;
+    drmModeConnector *connector;
+    drmModeRes *resources;
+
+    drm_fd = drmOpen("i915", NULL);
+    resources = drmModeGetResources(drm_fd);
+
+    printf("count_resources: %d\n", resources->count_connectors);
+
+    /* look for an EDID property */
+    for(int j = 0; j <  resources->count_connectors; j++)
+    {
+        conn_id = resources->connectors[j];
+        connector = drmModeGetConnector(drm_fd, conn_id);
+
+        for (int i = 0; i < connector->count_props; i++) {
+
+            drmModePropertyPtr props;
+            props = drmModeGetProperty(drm_fd, connector->props[i]);
+
+            if (!props)
+                continue;
+
+            if (!strcmp(props->name, "DPMS")) {
+                drmModeConnectorSetProperty(
+                    drm_fd, conn_id, connector->props[i], DRM_MODE_DPMS_OFF);
+
+                drmModeConnectorSetProperty(
+                    drm_fd, conn_id, connector->props[i], DRM_MODE_DPMS_ON);
+
+                printf("Set DPMS prop\n");
+            }
+            drmModeFreeProperty(props);
+        }
+    }
+	close(drm_fd);
+}
+
+static void
+usage(FILE *fp, int argc,char **argv)
+{
+	fprintf (fp,
+		 "Usage: %s [options]\n\n"
+		 "Options:\n"
+		 "-d | --device name   Video device name. Default value is /dev/video0\n"
+		 "-t | --test path of 'testdisplay'   Run the tool testdisplay\n"
+		 "-h | --help		   Print this message\n"
+		 "-p | --pre-view	   Preview the captured image for location real time\n"
+		 "",
+	argv[0]);
+}
+
+static const char short_options [] = "t:d:hmpAO:D:P:T";
+
+static const struct option
+long_options [] = {
+		{ "device",		required_argument,		NULL,			'd' },
+		{ "test",		required_argument,		NULL,			't' },
+		{ "help",		no_argument,			NULL,			'h' },
+		{ "preview",	no_argument,			NULL,			'p' },
+		{ "all",	no_argument,			NULL,			'A' },
+		{ "depth",	required_argument,			NULL,			'D' },
+		{ "connector ID",	required_argument,			NULL,			'O' },
+		{ "overlay",	required_argument,			NULL,			'P' },
+		{ "tiled",	no_argument,			NULL,			'T' },
+		{ 0, 0, 0, 0 }
+};
+
+char argvs[5][30];
+char *const testdisplay_argv[6] = 
+	{argvs[0], argvs[1], argvs[2], argvs[3],argvs[4], (char*)0};
+int testdisplay_argc = 0;
+
+int main(int argc,char *argv[])
+{
+	readlink("/proc/self/exe", QR4Ddir, sizeof(QR4Ddir) );
+
+	dpms_on();
+
+	char *pos = NULL;
+	pos = strrchr ( QR4Ddir, '/');
+	*(pos + 1) = '\0';
+
+	sprintf(testdisplay_argv[testdisplay_argc], "./testdisplay");
+	testdisplay_argc++;
+
+	for (;;) {
+		int index;
+		int c;
+		
+		c = getopt_long (argc, argv,
+						 short_options, long_options,
+						 &index);
+
+		if (-1 == c)
+				break;
+
+		switch (c) {
+		case 0: /* getopt_long() flag */
+				break;
+
+		case 'd':
+				memcpy(dev_names[0], optarg, strlen(optarg));
+				dev_count = 1;
+				break;
+
+		case 't':
+				run_test = 1;
+				child_exist = 1;
+				memcpy(testdisplaydir, optarg, strlen(optarg));
+				break;
+
+		case 'p':
+				pre_view = 1;
+				break;
+		case 'A':
+				sprintf(testdisplay_argv[testdisplay_argc], "-a");
+				testdisplay_argc++;
+				break;
+		case 'D':
+				sprintf(testdisplay_argv[testdisplay_argc], "-d %s", optarg);
+				testdisplay_argc++;
+				break;
+		case 'O':
+				sprintf(testdisplay_argv[testdisplay_argc], "-o %s", optarg);
+				testdisplay_argc++;
+				break;
+		case 'P':
+				sprintf(testdisplay_argv[testdisplay_argc], "-p %s", optarg);
+				testdisplay_argc++;
+				break;
+		case 'T':
+				sprintf(testdisplay_argv[testdisplay_argc], "-t");
+				testdisplay_argc++;
+				break;
+
+		case 'h':
+				usage (stdout, argc, argv);
+				exit (EXIT_SUCCESS);
+
+		default:
+				usage (stderr, argc, argv);
+				exit (EXIT_FAILURE);
+		}
+	}
+	sprintf(testdisplay_argv[testdisplay_argc], "-r");
+	testdisplay_argc++;
+
+	if ( run_test )
+		pid_test = fork();
+
+	if ( run_test && pid_test < 0)
+	{
+		printf("fail to create sub process");
+		exit(-1);
+	}
+
+	if (run_test && pid_test == 0)
+	{
+		char pathfile[256];
+		sprintf(pathfile, "%s/testdisplay", testdisplaydir);
+
+		chdir(testdisplaydir);
+
+		int ret = execv(pathfile, (char* const*)testdisplay_argv);
+
+		if(ret == -1)
+			errno_exit("execl");
+	}
+	else
+	{
+		get_devices();
+
+		//dev_count = 1;
+		for (int i = 0; i < dev_count; i++)
+		{
+			dev_fds[i] = open_device ( dev_names[i]);
+	
+			init_device (i );
+	
+//			start_capturing ( i );
+		}
+	
+		mainloop ();
+	
+		for (int i = 0; i < dev_count; i++)
+		{
+//			stop_capturing ( i );
+	
+			uninit_device ( i );
+	
+			close_device ( i );
+		}
+	}
+
+	return(0);
+}
+