FloatImg4PythonBinding/v4l2/funcs.c

/*
 *	V4L2 functions - ugly source code
 */

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

#include <fcntl.h>              /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include <linux/videodev2.h>

/* --------------------------------------------------------------------- */

extern int		verbosity;

    enum io_method {
	    IO_METHOD_READ,
	    IO_METHOD_MMAP,
	    IO_METHOD_USERPTR,
    };

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

    static char            *dev_name;
    static enum io_method   io = IO_METHOD_MMAP;
    static int              fd = -1;
    struct buffer          *buffers;
    static unsigned int     n_buffers;
    static int              out_buf;
    static int              force_format;
    static int              frame_count = 70;

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 fh, int request, void *arg)
{
int r;

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

return r;
}

/* --------------------------------------------------------------------- */
/*
 *
 */
int open_device(char *device)
{
struct stat		st;

if (-1 == stat(device, &st)) {
	fprintf(stderr, "Cannot identify '%s': %d, %s\n",
			device, errno, strerror(errno));
	exit(EXIT_FAILURE);
	}

if (!S_ISCHR(st.st_mode)) {
	fprintf(stderr, "%s is no device\n", device);
	exit(EXIT_FAILURE);
	}

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

if (-1 == fd) {
	fprintf(stderr, "Cannot open '%s': %d, %s\n",
			device, errno, strerror(errno));
	exit(EXIT_FAILURE);
	}

dev_name = strdup(device);		/* XXX */

return fd;
}
/* --------------------------------------------------------------------- */
    static void init_read(unsigned int buffer_size)
    {
	    buffers = calloc(1, sizeof(*buffers));

	    if (!buffers) {
		    fprintf(stderr, "Out of memory\n");
		    exit(EXIT_FAILURE);
	    }

	    buffers[0].length = buffer_size;
	    buffers[0].start = malloc(buffer_size);

	    if (!buffers[0].start) {
		    fprintf(stderr, "Out of memory\n");
		    exit(EXIT_FAILURE);
	    }
    }

    static void init_mmap(void)
    {
	    struct v4l2_requestbuffers req;

	    memset(&req, 0, sizeof(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;

		    memset(&buf, 0, sizeof(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");
	    }
    }

    static void init_userp(unsigned int buffer_size)
    {
	    struct v4l2_requestbuffers req;

	    memset(&req, 0, sizeof(req));

	    req.count  = 4;
	    req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	    req.memory = V4L2_MEMORY_USERPTR;

	    if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req)) {
		    if (EINVAL == errno) {
			    fprintf(stderr, "%s does not support "
				     "user pointer i/o\n", dev_name);
			    exit(EXIT_FAILURE);
		    } else {
			    errno_exit("VIDIOC_REQBUFS");
		    }
	    }

	    buffers = calloc(4, sizeof(*buffers));

	    if (!buffers) {
		    fprintf(stderr, "Out of memory\n");
		    exit(EXIT_FAILURE);
	    }

	    for (n_buffers = 0; n_buffers < 4; ++n_buffers) {
		    buffers[n_buffers].length = buffer_size;
		    buffers[n_buffers].start = malloc(buffer_size);

		    if (!buffers[n_buffers].start) {
			    fprintf(stderr, "Out of memory\n");
			    exit(EXIT_FAILURE);
		    }
	    }
    }
/* --------------------------------------------------------------------- */
int init_device(int notused)
{
struct v4l2_capability		cap;
struct v4l2_cropcap	cropcap;
struct v4l2_crop	crop;
struct v4l2_format	fmt;
unsigned int		min;

#if DEBUG_LEVEL
fprintf(stderr, ">>> %s ( %d )\n", __func__, notused);
#endif

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

	switch (io) {
	case IO_METHOD_READ:
		if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
			fprintf(stderr, "%s do not support read i/o\n", dev_name);
			exit(EXIT_FAILURE);
			}
		if (verbosity)	fprintf(stderr, "%s io method read OK\n", __func__);
		break;

	case IO_METHOD_MMAP:
	case IO_METHOD_USERPTR:
		if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
			fprintf(stderr, "%s do not support streaming i/o\n", dev_name);
			exit(EXIT_FAILURE);
			}
		if (verbosity)	fprintf(stderr, "%s io mmap/userptr OK\n", __func__);
		break;
		}		/* end switch */

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


	    memset(&cropcap, 0, sizeof(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. */
	    }


	memset(&fmt, 0, sizeof(fmt));			// CLEAR(fmt);

	    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	    if (force_format) {
		    fmt.fmt.pix.width       = 640;
		    fmt.fmt.pix.height      = 480;
		    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. */
	    } else {
		    /* Preserve original settings as set by v4l2-ctl for example */
		    if (-1 == xioctl(fd, VIDIOC_G_FMT, &fmt))
			    errno_exit("VIDIOC_G_FMT");
	    }

	    /* 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;

	    switch (io) {
	    case IO_METHOD_READ:
		    init_read(fmt.fmt.pix.sizeimage);
		    break;

	    case IO_METHOD_MMAP:
		    init_mmap();
		    break;

	    case IO_METHOD_USERPTR:
		    init_userp(fmt.fmt.pix.sizeimage);
		    break;
	    }
return 0;
}