display a picture on lcd by V4L2

[yu he]

Hi!
I want to display a picture on lcd,I use V4L2(which is one of linux interface) to achieve it,but is still some
problems.I need an example to study.
Here is my C source codes.Thanks for your help.
In this codes,I capture some pictures and it is correct,but displaying these pictures on lcd is wrong.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <assert.h> 

#include <getopt.h>             /* getopt_long() */

#include <fcntl.h>              /* low-level i/o */

#include <malloc.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <linux/videodev.h>
#include <asm/types.h>          /* for videodev2.h */
#include <linux/videodev2.h>
#include <linux/fb.h>


#define CLEAR(x) memset (&(x), 0, sizeof (x)) 


static int fd=-1;
static int fb=-1;
static int ret;


FILE *fp;  
char *filename = "image.yuv";


static unsigned char *fbmem;
static unsigned int screensize;
static unsigned int fb_width;
static unsigned int fb_height;
static unsigned int fb_depth;


static char *dev_name = NULL;
static unsigned int n_buffers = 0;	
static unsigned int width = 720;
static unsigned int height = 480;


struct buffer {    
       void *start;    
       size_t  length;
};

struct buffer *buffers = NULL;


struct timeval tv0,tv1;
unsigned int timeus=0;
float tm;


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  *fb_mmap(int fb, unsigned int screensize)
{
       caddr_t fbmem;
 
       if ((fbmem = mmap(NULL, screensize, PROT_READ | PROT_WRITE, MAP_SHARED, fb, 0)) == MAP_FAILED) 
          { errno_exit("fb_mmap"); }
       return (fbmem);
}


int fb_stat(int fb, int *width, int *height, int *depth)
{
       struct fb_fix_screeninfo fix;
       if (xioctl(fb, FBIOGET_FSCREENINFO, &fix))  { errno_exit("FBIOGET_FSCREENINFO"); }
	   printf("********************** fb_fix Information **********************\n");
	   printf(" Xpanstep: %d\n", fix.xpanstep);
	   printf(" Line length: %d\n", fix.line_length);
	   printf(" Physical Address: %x\n", fix.smem_start);
	   printf(" Buffer Length: %d\n", fix.smem_len);
	  

	   struct fb_var_screeninfo var;
       if (xioctl(fb, FBIOGET_VSCREENINFO, &var))  { errno_exit("FBIOGET_VSCREENINFO"); }
	   printf("********************** fb_var Information **********************\n");
	   printf(" xres: %d\n", var.xres);
	   printf(" yres: %d\n", var.yres);
	   printf(" bites_per_pixel: %d\n", var.bits_per_pixel);
	   printf(" memory_picture_width: %d\n", var.width);
	   printf(" memory_picture_height: %d\n", var.height);
 
       *width = var.xres;
       *height = var.yres;
       *depth = var.bits_per_pixel;
	   printf("********************** fb_stat Information **********************\n");
       printf(" width = %d\n height = %d\n depth = %d\n",*width,*height,*depth);
       return 0;
}


void fb_init()
{
   fb = open("/dev/fb0", O_RDWR);
   if(fb == -1)   { errno_exit("display device"); }
   else   { printf("*********************** Open video display success **********************\n"); }
 
   fb_stat(fb, &fb_width, &fb_height, &fb_depth);
   screensize = fb_width * fb_height * fb_depth / 8;
   fbmem = fb_mmap(fb, screensize);
}


int yuv_to_rgb16(unsigned char y, unsigned char u, unsigned char v, unsigned char *rgb)
{
    int r,g,b;
    int z;
    int rgb16;

    z = 0;
    r = 1.164 * (y - 16) + 1.596 * (v - 128);
    g = 1.164 * (y - 16) - 0.813 * (v - 128) - 0.391 * (u -128);
    b = 1.164 * (y - 16) + 2.018 * (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;

    rgb16 = (int)(((r >> 3)<<11) | ((g >> 2) << 5)| ((b >> 3) << 0));
    *rgb = (unsigned char)(rgb16 & 0xFF);
    rgb++;
    *rgb = (unsigned char)((rgb16 & 0xFF00) >> 8);
    return 0;
}


int image_convert(unsigned char *in_buf, unsigned char *rgb, int width, int height)
{
    int x,y,z=0;
    int blocks;
    unsigned char Y1, Y2, U, V;
    blocks = (width * height) * 2;

    for (y = 0; y < blocks; y+=4) 
		{
          Y1 = in_buf[y + 0];
		  U =  in_buf[y + 1];
		  Y2 = in_buf[y + 2];
		  V =  in_buf[y + 3];

          yuv_to_rgb16(Y1, U, V, &rgb[y]);
		  yuv_to_rgb16(Y2, U, V, &rgb[y + 2]);
        }
   return 0;
}


int process_image_lcd(unsigned char *rgb565)
{
     int x,y;
     unsigned char low_byte,hig_byte;
     unsigned short color;
     x=y=0;
     for (y=0;y<height;y++)
         {
           for (x=0;x<width;x++)
               {
                 low_byte=*rgb565;
                 rgb565++;
                 hig_byte=*rgb565;                                  
                 color= (unsigned short)(low_byte+hig_byte *256);
                 fbmem[y*width+x] =color;
                 rgb565++;
               }
         }
     return 0;        
}


static void process_image_fp(const void *p, int size)
{   
	fwrite(p, size, 1, fp);
}


static int read_frame(void)  
{  
    unsigned int i;
    struct v4l2_buffer buf; 
	CLEAR (buf);
	buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf.memory = V4L2_MEMORY_MMAP;
    if (-1 == xioctl (fd, 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 < n_buffers); 
		
    process_image_fp(buffers[buf.index].start, buf.length);

    int data_buf_len;
    char *data_buffer;
    int conver_result;
    data_buf_len=width*height*2;
    data_buffer=(unsigned char *)malloc(data_buf_len);
    memset(data_buffer,0,data_buf_len);   
    conver_result=image_convert(buffers[buf.index].start,data_buffer,width,height);
    if (conver_result<0)  { errno_exit("conver_result"); }

    process_image_lcd(data_buffer);
	
	if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))  errno_exit ("VIDIOC_QBUF");
	return 1;
}  

static void mainloop(void)
{  
   while(1)
   	    { 
           gettimeofday(&tv0, NULL );
           for (;;)
 	           { 
                  fd_set fds; 
		          struct timeval tv;
                  int r;
                  FD_ZERO(&fds);  			
                  FD_SET(fd, &fds);  	
                  tv.tv_sec = 2;
	              tv.tv_usec = 0;
                  r = select(fd + 1, &fds, NULL, NULL, &tv);  
		          if (-1 == r) 
		  	         {
                        if (EINTR == errno)  continue;
                        errno_exit ("select");
                     }
                  if (0 == r) 
		  	         {
                        fprintf (stderr, "select timeout\n");
                        exit (EXIT_FAILURE);
                     }		  
	              if (read_frame())  break;
           	   }     
           gettimeofday(&tv1, NULL ); 
	       timeus = (1000000 * ( tv1.tv_sec - tv0.tv_sec ) + tv1.tv_usec - tv0.tv_usec); 
		   tm=timeus/1000000.0;
		   printf(">>>>>> time = %f s\n",tm);
   	    }
}  


static void start_capturing(void)
{	
	unsigned int i;
    enum v4l2_buf_type type;
    for (i = 0; i < n_buffers; ++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 (fd, VIDIOC_QBUF, &buf))  errno_exit ("VIDIOC_QBUF");
        }
	type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))  errno_exit ("VIDIOC_STREAMON"); 
}

static void init_mmap(void)
{
    struct v4l2_requestbuffers req;
	CLEAR(req);
	req.count=4; 
	req.type=V4L2_BUF_TYPE_VIDEO_CAPTURE; 
	req.memory=V4L2_MEMORY_MMAP;
	if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) 
	   {
          if (EINVAL == errno) 
		  	 {
                fprintf (stderr, "%s does not support " "memory mapping\n", dev_name); 
				exit (EXIT_FAILURE);
             } 
		  else 
		  	 {
                errno_exit ("VIDIOC_REQBUFS");
             }
       }
	if (req.count < 2) 
	   {
          fprintf (stderr, "Insufficient buffer memory on %s\n", dev_name);
          exit (EXIT_FAILURE);
       }
    buffers = calloc(req.count, sizeof(*buffers));
	if (!buffers)  
       {  
		  fprintf (stderr, "Out of memory\n");
		  exit (EXIT_FAILURE);
	   }
    for (n_buffers = 0; n_buffers < req.count; ++n_buffers)
        {    
           struct v4l2_buffer buf;		  
		   CLEAR(buf);    
		   buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;    
		   buf.memory = V4L2_MEMORY_MMAP;    
		   buf.index = n_buffers;
		   if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf))  errno_exit ("VIDIOC_QUERYBUF");
		   buffers[n_buffers].length = buf.length;
		   buffers[n_buffers].start = mmap (NULL /* start anywhere */,
									        buf.length,
									        PROT_READ | PROT_WRITE /* required */,
									        MAP_SHARED /* recommended */,
									        fd, buf.m.offset);
		   
           if (MAP_FAILED == buffers[n_buffers].start)  errno_exit ("mmap");
        }
}


int init_capture_device()
{   
    struct v4l2_capability cap;
    if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap)) 
	   {
		  if (EINVAL == errno) 
		  	 {
			    fprintf (stderr, "%s is no V4L2 device\n", dev_name);
				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_name);
		  exit (EXIT_FAILURE);
	   }
	printf("*********************** Capability Informations *************************\n");
    printf(" driver: %s\n", cap.driver);
    printf(" card: %s\n", cap.card);
    printf(" bus_info: %s\n", cap.bus_info);
    printf(" version: %u.%u.%u\n", (cap.version >> 16) & 0xFF,(cap.version >> 8) & 0xFF,cap.version & 0xFF);
    printf(" capabilities: %08x\n", cap.capabilities);


	struct v4l2_cropcap cropcap;
	struct v4l2_crop crop;
    CLEAR(cropcap);
	cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (0 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) 
	   {
		  crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		  crop.c = cropcap.defrect; /* reset to default */
	      if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) 
		  	 {
			    switch (errno) 
					   {
						  case EINVAL:
									    /* Cropping not supported. */
									    break;
						  default:
								        /* Errors ignored. */
									    break;
					   }
			 }
	   } 
	else 
	   {		
		  printf("**************** Cropcap can't use ****************\n");
	   }

	
	struct v4l2_input input;
	unsigned int i = 0;
	while (1) 
		  {
		     input.index = i;
		     ret = ioctl(fd, VIDIOC_ENUMINPUT, &input);
		     if (ret < 0)  break;
		     printf (" inputname: %s\n", input.name);
		     i++;
	      }

	CLEAR(input);		  
	if (-1 == xioctl (fd, VIDIOC_G_INPUT, &input.index))   { errno_exit ("VIDIOC_G_INPUT");} 
	if (-1 == xioctl (fd,VIDIOC_ENUMINPUT, &input))	 { errno_exit ("VIDIOC_ENUMINPUT");} 
	
	printf("************************ Input information ************************\n");
	printf (" current inputname: %s\n", input.name);
	switch (input.type) {  
						  case V4L2_INPUT_TYPE_TUNER:	   printf(" V4L2_INPUT_TYPE_TUNER\n");
	                      case V4L2_INPUT_TYPE_CAMERA:	   printf(" V4L2_INPUT_TYPE_CAMERA\n");
						} 
	printf (" status: %0x\n", input.status); 


	struct v4l2_fmtdesc fmtdesc;	
	CLEAR(fmtdesc);
	fmtdesc.index = 0;
	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	printf("********************* Support Format Informations **********************\n");
	while ((ret = xioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc)) == 0) 
	      {
			fmtdesc.index++;
			printf(" pixelformat ''%c%c%c%c'', description ''%s''\n",
					  fmtdesc.pixelformat & 0xFF, (fmtdesc.pixelformat >> 8) & 0xFF, (fmtdesc.pixelformat >> 16) & 0xFF, 
					  (fmtdesc.pixelformat >> 24) & 0xFF, fmtdesc.description);
	      }


	unsigned int min;
    struct v4l2_format fmt;   
	CLEAR (fmt);
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	fmt.fmt.pix.width= width;
	fmt.fmt.pix.height= height;
	fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
	fmt.fmt.pix.field= V4L2_FIELD_INTERLACED;
	if (-1 == xioctl (fd, 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;


    if(xioctl(fd, VIDIOC_G_FMT, &fmt) == -1)  { errno_exit("VIDIOC_G_FMT"); }

    printf("********************* Stream Format Informations **********************\n");
	printf(" current format type: %d\n", fmt.type);
    printf(" current format width: %d\n", fmt.fmt.pix.width);
    printf(" current format height: %d\n", fmt.fmt.pix.height);
	char fmtstr[8];
	memset(fmtstr, 0, 8);
	memcpy(fmtstr, &fmt.fmt.pix.pixelformat, 4);
	printf(" pixelformat: %s\n", fmtstr);
	printf(" field: %d\n", fmt.fmt.pix.field);
	printf(" bytesperline: %d\n", fmt.fmt.pix.bytesperline);
	printf(" sizeimage: %d\n", fmt.fmt.pix.sizeimage);
	printf(" colorspace: %d\n", fmt.fmt.pix.colorspace);
	printf(" priv: %d\n", fmt.fmt.pix.priv);
}

static void open_capture_device(void)
{
        struct stat st; 

        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);
        }
}


int main(int argc, char **argv)
{
   printf("*************************** Program start ***************************\n");

   dev_name = "/dev/video0";
   
   open_capture_device();
   
   init_capture_device();

   init_mmap();
   
   start_capturing();
   
   fb_init(); 

   fp = fopen(filename, "wa+");
   
   mainloop();
   
   return 0;
}