V4L2摄像头驱动demo

// extra_camera.h

#ifndef __ExtraCamera_H__
#define __ExtraCamera_H__ 

#define CAMERA_WIDTH    (1280)
#define CAMERA_HEIGH    (720)

#undef TAG
#define TAG "ExtraCamera debug ---"

#define DEBUG_TAG "ExtraCamera debug --- "
#define DEBUGPRINT(fmt, args...) \
    do { \
        printf(DEBUG_TAG "[%s][%d] --- " fmt "\n",__FUNCTION__,__LINE__, ##args); \
    } while (0)

#define ERR_RETURN(fmt, args...) \
    do { DEBUGPRINT(fmt, ##args); return -1; } while (0)

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


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

class ExtraCamera {
private:
	char		dev_name[64];
	int		fd;
	struct buffer * buffers;
	unsigned int 	n_buffers;

	int xioctl(int fd, int request, void * arg);
	int do_handle_frame(int(*handler)(unsigned char *, int,void *),void * extra);
	int start_capturing(void);
	int stop_capturing(void);
	int init_mmap(void);
	int fini_mmap(void);
	int init_device(void);
	int fini_device(void);
	int open_device(void);
	int close_device(void);
	

public:
	ExtraCamera(const char *device_name);
	~ExtraCamera();
	int init();
	int fini();
	int handle_frame(int(*handler)(unsigned char *, int,void *),void * extra);
	int cameraWidth;
	int cameraHeight;
};

#endif /* __ExtraCamera_H__ */

// extra_camera.cpp

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#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 "extra_camera.h"

int ExtraCamera::xioctl(int fd, int request, void * arg)
{
    int r;

    do {
        r = ioctl(fd, request, arg);
    } while(-1 == r && EINTR == errno);

    return r;
}

int ExtraCamera::do_handle_frame(int(*handler)(unsigned char *, int,void *),void *extra)
{
    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:
                ERR_RETURN("VIDIOC_DQBUF, errno: %d", errno);
        }
    }
    DEBUGPRINT("extra == %s",extra);
    //assert(buf.index < n_buffers);
	if (buffers)
    	handler((unsigned char *)buffers[buf.index].start, cameraWidth*cameraHeight*2,extra);
	else
		ERR_RETURN("buffers == NULL");


    if(-1 == xioctl(fd, VIDIOC_QBUF, &buf))
        ERR_RETURN("VIDIOC_QBUF");

    return 0;
}

int ExtraCamera::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))
            ERR_RETURN("VIDIOC_QBUF");
    }
    
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if(-1 == xioctl(fd, VIDIOC_STREAMON, &type))
        ERR_RETURN("VIDIOC_STREAMON");

    return 0;
}

int ExtraCamera::stop_capturing(void)
{
    enum v4l2_buf_type type;
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

    if(-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
        ERR_RETURN("VIDIOC_STREAMOFF");

    return 0;
}

int ExtraCamera::init_mmap(void)
{
    struct v4l2_requestbuffers req;
    CLEAR(req);

    req.count               = 2;
    req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory              = V4L2_MEMORY_MMAP;

    if(-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
        if(EINVAL == errno)
            ERR_RETURN("%s does not support memory mapping\n", dev_name);
        else
            ERR_RETURN("VIDIOC_REQBUFS");
    }

    if(req.count < 2)
        ERR_RETURN("Insufficient buffer memory on %s\n", dev_name);

    buffers = (struct buffer *)calloc(req.count, sizeof(*buffers));

    if(!buffers)
        ERR_RETURN("Out of memory\n");

    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))
            ERR_RETURN("VIDIOC_QUERYBUF");

        DEBUGPRINT("buf.length %d\n",buf.length);
        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)
            ERR_RETURN("mmap");
    }

    return 0;
}

int ExtraCamera::fini_mmap(void)
{
    unsigned int i;

    for(i = 0; i < n_buffers; ++i)
		if (MAP_FAILED != buffers[i].start && 0x0 != buffers[i].start)
        	if(-1 == munmap(buffers[i].start, buffers[i].length))
            	ERR_RETURN("munmap");

    free(buffers);
	buffers = NULL;
    return 0;
}

int ExtraCamera::init_device(void)
{
    struct v4l2_capability cap;
    struct v4l2_cropcap cropcap;
    struct v4l2_crop crop;
    struct v4l2_format fmt;
    unsigned int min;

    if(-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
        if(EINVAL == errno) {
            ERR_RETURN("%s is no V4L2 device\n", dev_name);
        } else {
            ERR_RETURN("VIDIOC_QUERYCAP");
        }
    }

    if(!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
        ERR_RETURN("%s is no video capture device\n", dev_name);

    if(!(cap.capabilities & V4L2_CAP_STREAMING))
        ERR_RETURN("%s does not support streaming i/o\n", dev_name);

    /* Select video input, video standard and tune here. */

    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 {    
        /* Errors ignored. */
    }

    CLEAR(fmt);

    fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    //写死分辨率
    cameraWidth = CAMERA_WIDTH;
    cameraHeight = CAMERA_HEIGH;

    fmt.fmt.pix.width       = cameraWidth; 
    fmt.fmt.pix.height      = cameraHeight;

    // fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;   //输出MJPEG
    fmt.fmt.pix.field       = V4L2_FIELD_NONE;

    if(-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
            ERR_RETURN("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;

    return 0;
}

int ExtraCamera::fini_device(void)
{
	/* do nothing for now */
	return 0;
}

int ExtraCamera::open_device(void)
{
    struct stat st; 

    if(-1 == stat(dev_name, &st))
        ERR_RETURN("Cannot identify '%s': %d, %s\n", dev_name, errno, strerror(errno));

    if(!S_ISCHR(st.st_mode))
        ERR_RETURN("%s is no device\n", dev_name);

    fd = open(dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);

    DEBUGPRINT("fd :%d\n",fd);

    if(-1 == fd)
        ERR_RETURN("Cannot open '%s': %d, %s\n", dev_name, errno, strerror(errno));

    return 0;
}

int ExtraCamera::close_device(void)
{
    if(-1 == close(fd))
        ERR_RETURN("close");

    fd = -1;
    return 0;
}

int ExtraCamera::init()
{
    int ret;

    ret = open_device();
    if (ret)
        ERR_RETURN();

    ret = init_device();
    if (ret)
        ERR_RETURN();

    ret = init_mmap();
    if (ret)
        ERR_RETURN();

    ret = start_capturing();
    if (ret)
        ERR_RETURN();

    return 0;
}

int ExtraCamera::fini()
{
    int ret;

    ret = stop_capturing();
    if (ret)
        ERR_RETURN();

    ret = fini_mmap();
    if (ret)
        ERR_RETURN();

    ret = fini_device();
    if (ret)
        ERR_RETURN();

    ret = close_device();
    if (ret)
        ERR_RETURN();

    return 0;
}

int ExtraCamera::handle_frame(int(*handler)(unsigned char *, int,void *),void *extra)
{
    fd_set fds;
    struct timeval tv;
    int r;

    FD_ZERO(&fds);
    FD_SET(fd, &fds);

    /* Timeout. */
    tv.tv_sec = 2;
    tv.tv_usec = 0;

    r = select(fd + 1, &fds, NULL, NULL, &tv);

    if(-1 == r && EINTR == errno)
        ERR_RETURN("select");

    if(0 == r)
        ERR_RETURN("select timeout\n");

    return do_handle_frame(handler,extra);

    /* EAGAIN - continue select loop. */
}


ExtraCamera::ExtraCamera(const char *device_name)
{
	snprintf(dev_name, sizeof(dev_name), "%s", device_name);
	fd = -1;
	buffers = NULL;
	n_buffers = 0;
}

ExtraCamera::~ExtraCamera()
{
	fd = -1;
	buffers = NULL;
	n_buffers = 0;
}

#if 1 /* debug */
unsigned char buf1[CAMERA_WIDTH*CAMERA_HEIGH*2];

int
save_file_1(unsigned char* data, int size,void *extra)
{
    
    char path[256];
    snprintf(path, 256, "/sdcard/from1.png");
    FILE* fp = fopen(path, "w+");
    if(fp == NULL){
        DEBUGPRINT("write failed.");
        return -1;
    }
    fwrite(data, size, 1, fp);
    fclose(fp);
    
    // memcpy(buf1, data, size);
    return 0;
}

unsigned char buf2[CAMERA_WIDTH*CAMERA_HEIGH*2];

int
save_file_2(unsigned char* data, int size,void *extra)
{
    
    char path[256];
    snprintf(path, 256, "/sdcard/from2.png");
    FILE* fp = fopen(path, "w+");
    if(fp == NULL){
        DEBUGPRINT("write failed.");
        return -1;
    }
    fwrite(data, size, 1, fp);
    fclose(fp);
    
    // memcpy(buf2, data, size);
    return 0;
}

int
merge_two_frame()
{
	char path[256];
	snprintf(path, 256, "/sdcard/from.cam.yuv");
	FILE* fp = fopen(path, "w+");
	if(fp == NULL){
        	DEBUGPRINT("write failed.");
        	return -1;
        }

	int i, j = 0, k = 0;
	for (i = 0; i < CAMERA_WIDTH*CAMERA_HEIGH; i++)
		if ((i % CAMERA_WIDTH) < (CAMERA_WIDTH / 2)) {
			fwrite((const void*)(&buf1[j*2*2]), 2, 1, fp);
			j++;
		} else {
			fwrite((const void*)(&buf2[k*2*2]), 2, 1, fp);
			k++;
		}
    fclose(fp);
    return 0;
}

int
main()
{
	int ret;

    ExtraCamera *ec1 = new ExtraCamera("/dev/video2");
    ret = ec1->init();
    if (ret)
        return -1;

    ExtraCamera *ec2 = new ExtraCamera("/dev/video3");
    ret = ec2->init();
    if (ret)
        return -1;


    for(;;) {
		int r1, r2;
        r1 = ec1->handle_frame(save_file_1,(void *)"/sdcard/from.cam.yuv");
		r2 = ec2->handle_frame(save_file_2,(void *)"/sdcard/from.cam.yuv");
		if (!r1 || !r2) {
			DEBUGPRINT("capture failed.");
			break;
		}
		// merge_two_frame();
		usleep(50*1000);
    }
    
    ec1->fini();
    delete ec1;

    ec2->fini();
    delete ec2;

    return 0;
}
#endif