Linux/AM5728: 8-bit raw encode issue

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <fcntl.h>
#include <errno.h>
#include <linux/videodev2.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>

#include <time.h>

struct v4l2_user_buffer {
    int index;
    int length;
    char *addr;
};
struct v4l2_user_buffer user_buffer[5];

int main(void)
{
    int fp;
    int i = 0;
    int status = 0;
    int v4l2_fd = 0;
    struct v4l2_format format;
    struct v4l2_requestbuffers requestbuffers;
    struct v4l2_buffer buffer;

    struct timeval tpstart;
    unsigned int timems =0, lasttime = 0;

    fp = open("frame.yuv", O_RDWR | O_CREAT | O_TRUNC);
    if (fp < 0) {
        printf("%d: fopen error\n", __LINE__);
        return -1;
    }

    v4l2_fd = open("/dev/video1", O_RDWR);
    if (v4l2_fd == -1) {
        printf("%d: open error\n", __LINE__);
        return -1;
    }

    memset(&format, 0x00, sizeof(struct v4l2_format));
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    format.fmt.pix.width = 640;
    format.fmt.pix.height = 480;
    format.fmt.pix.field = V4L2_FIELD_NONE;
    format.fmt.pix.pixelformat = V4L2_PIX_FMT_NV12;
    status = ioctl(v4l2_fd, VIDIOC_S_FMT, &format);
    if (status < 0) {
        printf("%d: ioctl VIDIOC_S_FMT error\n", __LINE__);
        return -1;
    }

    memset(&format, 0x00, sizeof(struct v4l2_format));
    format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    status = ioctl(v4l2_fd, VIDIOC_G_FMT, &format);
    if (status < 0) {
        printf("%d: ioctl VIDIOC_G_FMT error\n", __LINE__);
        return -1;
    }
    printf("format width: [%d]\n", format.fmt.pix.width);
    printf("format height: [%d]\n", format.fmt.pix.height);

    memset(&requestbuffers, 0x00, sizeof(struct v4l2_requestbuffers));
    requestbuffers.count = 2;// max is 5
    requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    requestbuffers.memory = V4L2_MEMORY_MMAP;
    status = ioctl(v4l2_fd, VIDIOC_REQBUFS, &requestbuffers);
    if (status < 0) {
        printf("%d: ioctl VIDIOC_REQBUFS error\n", __LINE__);
        return -1;
    }


    for (i = 0; i < requestbuffers.count ; i++) {
        buffer.index = i;
        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buffer.memory = V4L2_MEMORY_MMAP;
        status = ioctl(v4l2_fd, VIDIOC_QUERYBUF, &buffer);
        if (status < 0) {
            printf("%d: ioctl VIDIOC_QUERYBUF error\n", __LINE__);
            return -1;
        }

        user_buffer[i].index = i;
        user_buffer[i].length = buffer.length;
        user_buffer[i].addr = (char *)mmap(NULL,
                buffer.length,
                PROT_READ | PROT_WRITE,
                MAP_SHARED,
                v4l2_fd,
                buffer.m.offset);
        if (user_buffer[i].addr == MAP_FAILED) {
            printf("%d: mmap error\n", __LINE__);
            return -1;
        }
        printf("user_buffer index: [%d]\n", user_buffer[i].index);
        printf("user_buffer length: [%d]\n", user_buffer[i].length);
        printf("user_buffer addr: [%p]\n", user_buffer[i].addr);
    }

    for (i = 0; i < requestbuffers.count ; i ++) {
        buffer.index = i;
        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buffer.memory = V4L2_MEMORY_MMAP;
        status = ioctl(v4l2_fd, VIDIOC_QBUF, &buffer);
        if (status < 0) {
            printf("%d: ioctl VIDIOC_QBUF error\n", __LINE__);
            return -1;
        }
    }

    int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    status = ioctl(v4l2_fd, VIDIOC_STREAMON, &type);
    if (status < 0) {
        printf("%d: ioctl VIDIOC_STREAMOFF error\n", __LINE__);
        return -1;
    }

    for (i = 0 ; i < 10; i++) {

        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buffer.memory = V4L2_MEMORY_MMAP;
        status = ioctl(v4l2_fd, VIDIOC_DQBUF, &buffer);
        if (status < 0) {
            printf("%d: ioctl VIDIOC_DQBUF error\n", __LINE__);
            return -1;
        }

#if 1
        printf("buf index  %d len %d get frame %d :",buffer.index ,user_buffer[buffer.index].length ,i);
        gettimeofday(&tpstart,NULL);
        timems = (tpstart.tv_sec * 1000) + (tpstart.tv_usec / 1000) ;
        printf("len %d time %u user time %u \n",user_buffer[buffer.index].length, timems ,timems - lasttime);
        lasttime = timems;
#endif
        write(fp,user_buffer[buffer.index].addr,user_buffer[buffer.index].length);

        buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buffer.memory = V4L2_MEMORY_MMAP;
        buffer.m.offset = (unsigned long)user_buffer[buffer.index].addr;
        status = ioctl(v4l2_fd, VIDIOC_QBUF, &buffer);
        if (status < 0) {
            printf("%d: ioctl VIDIOC_DQBUF error\n", __LINE__);
            return -1;
        }

    }

    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    status = ioctl(v4l2_fd, VIDIOC_STREAMOFF, &type);
    if (status < 0) {
        printf("%d: ioctl VIDIOC_STREAMOFF error\n", __LINE__);
        return -1;
    }

    close (v4l2_fd);
    close(fp);
    return status;
}