AM62P: CSI-RX frame repetition

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include <linux/types.h>          /* for videodev2.h */
#include <linux/videodev2.h>
#include <poll.h>
#include <sys/mman.h>
#include <stdlib.h>


char *videodev[4];

int main(int argc, char **argv)
{
    int fd[4];
    struct v4l2_fmtdesc fmtdesc;
    struct v4l2_frmsizeenum fsenum;
    int fmt_index = 0;
    int frame_index = 0;
    int i;
    void *bufs[4][32];
    int buf_cnt;
    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    struct pollfd fds[4];
    char filename[32];
    int file_cnt[4] = {0,0,0,0};
	int ret;
	
	videodev[0] = "/dev/video2";
	videodev[1] = "/dev/video3";
	videodev[2] = "/dev/video4";
	videodev[3] = "/dev/video5";

	int k;
	for (k = 0; k < 4; k++) {
	    /* open */
	    fd[k] = open(videodev[k], O_RDWR);
	    if ( fd[k] < 0) {
	        printf("can not open %s\n", videodev[k]);
	        return -1;
	    }
		printf("open %s ok\n", videodev[k]);

		
	    struct v4l2_capability cap;
	    memset(&cap, 0, sizeof(struct v4l2_capability));

	    struct v4l2_format fmt;
	    memset(&fmt, 0, sizeof(struct v4l2_format));
	    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	    fmt.fmt.pix.width = 1280;
	    fmt.fmt.pix.height = 964;
	    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_UYVY;
	    fmt.fmt.pix.field = V4L2_FIELD_ANY;
	    if (0 == ioctl(fd[k], VIDIOC_S_FMT, &fmt))
	    {
	        printf("%s,  set format ok: %d x %d\n",videodev[k], fmt.fmt.pix.width, fmt.fmt.pix.height);
	    }
	    else
	    {
	        printf("%s,can not set format\n",videodev[k]);
	        return -1;
	    }

	    struct v4l2_requestbuffers rb;
	    memset(&rb, 0, sizeof(struct v4l2_requestbuffers));
	    rb.count = 32;
	    rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	    rb.memory = V4L2_MEMORY_MMAP;

	    if (0 == ioctl(fd[k], VIDIOC_REQBUFS, &rb))
	    {
	        buf_cnt = rb.count;
	        for(i = 0; i < rb.count; i++) {
	            struct v4l2_buffer buf;
	            memset(&buf, 0, sizeof(struct v4l2_buffer));
	            buf.index = i;
	            buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	            buf.memory = V4L2_MEMORY_MMAP;
	            if (0 == ioctl(fd[k], VIDIOC_QUERYBUF, &buf))
	            {
	                /* mmap */
	                bufs[k][i] = mmap(0 /* start anywhere */ ,
	                                  buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd[k],
	                                  buf.m.offset);
	                if(bufs[k][i] == MAP_FAILED) {
	                    perror("Unable to map buffer");
	                    return -1;
	                }
	            }
	            else
	            {
	                printf("can not query buffer\n");
	                return -1;
	            }            
	        }

	        printf("map %d buffers ok\n", buf_cnt);
	        
	    }
	    else
	    {
	        printf("can not request buffers\n");
	        return -1;
	    }

	    for(i = 0; i < buf_cnt; ++i) {
	        struct v4l2_buffer buf;
	        memset(&buf, 0, sizeof(struct v4l2_buffer));
	        buf.index = i;
	        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	        buf.memory = V4L2_MEMORY_MMAP;
	        if (0 != ioctl(fd[k], VIDIOC_QBUF, &buf))
	        {
	            perror(" Unable to queue buffer");
	            return -1;
	        }
	    }
	    printf("queue buffers ok\n");

	    if (0 != ioctl(fd[k], VIDIOC_STREAMON, &type))
	    {
	        perror("Unable to start capture");
	        return -1;
	    }
	    printf("start capture ok\n");
	}
    while (1)
    {
		 struct v4l2_buffer buf;
	
        /* poll */
        memset(fds, 0, sizeof(fds));
		fds[0].fd = fd[0];
		fds[0].events = POLLIN;
		fds[1].fd = fd[1];
		fds[1].events = POLLIN;
		fds[2].fd = fd[2];
		fds[2].events = POLLIN;
		fds[3].fd = fd[3];
		fds[3].events = POLLIN;

		ret = poll(fds, 4, -1);
		if (ret > 0) {
			for (k = 0; k <4; k++) {
				if (fds[k].revents & POLLIN){
					memset(&buf, 0, sizeof(struct v4l2_buffer));
					buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
					buf.memory = V4L2_MEMORY_MMAP;
					if (0 != ioctl(fds[k].fd, VIDIOC_DQBUF, &buf)) {
		                perror("Unable to dequeue buffer");
		                return -1;
		            }
					
					/* save frame data */
					{
						sprintf(filename, "/tmp/video_%d_data%04d.yuv", k+2, file_cnt[k]);
						int fd_file = open(filename, O_RDWR | O_CREAT, 0666);
						if (fd_file < 0) {
							printf("can not create file : %s\n", filename);
						}
						write(fd_file, bufs[k][buf.index], buf.bytesused);
						close(fd_file);
					}

					file_cnt[k]++;
					
					if (0 != ioctl(fds[k].fd, VIDIOC_QBUF, &buf))
		            {
		                perror("Unable to queue buffer");
		                return -1;
		            }
				}
			}

		}
    }

    return 0;
}