omap3-isp-live: Initial commit

Signed-off-by: Laurent Pinchart <laurent.pinchart@ideasonboard.com>

1 files changed, 717 insertions, 0 deletions

diff --git a/isp/v4l2.c b/isp/v4l2.c
new file mode 100644
index 0000000..75e90b1
--- /dev/null
+++ b/isp/v4l2.c
@@ -0,0 +1,717 @@
+/*
+ * OMAP3 ISP library - V4L2 devices
+ *
+ * Copyright (C) 2010-2011 Ideas on board SPRL
+ *
+ * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com>
+ *
+ * This library is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU Lesser General Public License as published by
+ * the Free Software Foundation; either version 2.1 of the License, or (at
+ * your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
+ * more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public License
+ * along with this library; if not, write to the Free Software Foundation,
+ * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#include <stdio.h>
+#include <string.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <getopt.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <sys/select.h>
+#include <sys/time.h>
+
+#include <linux/videodev2.h>
+
+#include "list.h"
+#include "tools.h"
+#include "v4l2.h"
+
+#ifndef V4L2_BUF_FLAG_ERROR
+#define V4L2_BUF_FLAG_ERROR	0x0040
+#endif
+
+struct v4l2_ival_desc {
+	struct v4l2_fract min;
+	struct v4l2_fract max;
+	struct v4l2_fract step;
+
+	struct list_entry list;
+};
+
+struct v4l2_frame_desc {
+	unsigned int min_width;
+	unsigned int min_height;
+	unsigned int max_width;
+	unsigned int max_height;
+	unsigned int step_width;
+	unsigned int step_height;
+
+	struct list_entry list;
+	struct list_entry ivals;
+};
+
+struct v4l2_format_desc {
+	unsigned int pixelformat;
+
+	struct list_entry list;
+	struct list_entry frames;
+};
+
+/* -----------------------------------------------------------------------------
+ * Formats enumeration
+ */
+
+static int
+v4l2_enum_frame_intervals(struct v4l2_device *dev, struct v4l2_format_desc *format,
+	struct v4l2_frame_desc *frame)
+{
+	struct v4l2_ival_desc *ival;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; ; ++i) {
+		struct v4l2_frmivalenum ivalenum;
+
+		memset(&ivalenum, 0, sizeof ivalenum);
+		ivalenum.index = i;
+		ivalenum.pixel_format = format->pixelformat;
+		ivalenum.width = frame->min_width;
+		ivalenum.height = frame->min_height;
+		ret = ioctl(dev->fd, VIDIOC_ENUM_FRAMEINTERVALS, &ivalenum);
+		if (ret < 0)
+			break;
+
+		if (i != ivalenum.index)
+			printf("Warning: driver returned wrong ival index "
+				"%u.\n", ivalenum.index);
+		if (format->pixelformat != ivalenum.pixel_format)
+			printf("Warning: driver returned wrong ival pixel "
+				"format %08x.\n", ivalenum.pixel_format);
+		if (frame->min_width != ivalenum.width)
+			printf("Warning: driver returned wrong ival width "
+				"%u.\n", ivalenum.width);
+		if (frame->min_height != ivalenum.height)
+			printf("Warning: driver returned wrong ival height "
+				"%u.\n", ivalenum.height);
+
+		ival = malloc(sizeof *ival);
+		if (ival == NULL)
+			return -ENOMEM;
+
+		memset(ival, 0, sizeof *ival);
+
+		switch (ivalenum.type) {
+		case V4L2_FRMIVAL_TYPE_DISCRETE:
+			ival->min = ivalenum.discrete;
+			ival->max = ivalenum.discrete;
+			ival->step.numerator = 1;
+			ival->step.denominator = 1;
+			break;
+
+		case V4L2_FRMIVAL_TYPE_STEPWISE:
+			ival->min = ivalenum.stepwise.min;
+			ival->max = ivalenum.stepwise.max;
+			ival->step = ivalenum.stepwise.step;
+			break;
+
+		default:
+			printf("Error: driver returned invalid frame ival "
+				"type %u\n", ivalenum.type);
+			return -EINVAL;
+		}
+
+		list_append(&ival->list, &frame->ivals);
+	}
+
+	return 0;
+}
+
+static int
+v4l2_enum_frame_sizes(struct v4l2_device *dev, struct v4l2_format_desc *format)
+{
+	struct v4l2_frame_desc *frame;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; ; ++i) {
+		struct v4l2_frmsizeenum frmenum;
+
+		memset(&frmenum, 0, sizeof frmenum);
+		frmenum.index = i;
+		frmenum.pixel_format = format->pixelformat;
+
+		ret = ioctl(dev->fd, VIDIOC_ENUM_FRAMESIZES, &frmenum);
+		if (ret < 0)
+			break;
+
+		if (i != frmenum.index)
+			printf("Warning: driver returned wrong frame index "
+				"%u.\n", frmenum.index);
+		if (format->pixelformat != frmenum.pixel_format)
+			printf("Warning: driver returned wrong frame pixel "
+				"format %08x.\n", frmenum.pixel_format);
+
+		frame = malloc(sizeof *frame);
+		if (frame == NULL)
+			return -ENOMEM;
+
+		memset(frame, 0, sizeof *frame);
+
+		list_init(&frame->ivals);
+		frame->step_width = 1;
+		frame->step_height = 1;
+
+		switch (frmenum.type) {
+		case V4L2_FRMSIZE_TYPE_DISCRETE:
+			frame->min_width = frmenum.discrete.width;
+			frame->min_height = frmenum.discrete.height;
+			frame->max_width = frmenum.discrete.width;
+			frame->max_height = frmenum.discrete.height;
+			break;
+
+		case V4L2_FRMSIZE_TYPE_STEPWISE:
+			frame->step_width = frmenum.stepwise.step_width;
+			frame->step_height = frmenum.stepwise.step_height;
+		case V4L2_FRMSIZE_TYPE_CONTINUOUS:
+			frame->min_width = frmenum.stepwise.min_width;
+			frame->min_height = frmenum.stepwise.min_height;
+			frame->max_width = frmenum.stepwise.max_width;
+			frame->max_height = frmenum.stepwise.max_height;
+			break;
+
+		default:
+			printf("Error: driver returned invalid frame size "
+				"type %u\n", frmenum.type);
+			return -EINVAL;
+		}
+
+		list_append(&frame->list, &format->frames);
+
+		ret = v4l2_enum_frame_intervals(dev, format, frame);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+static int v4l2_enum_formats(struct v4l2_device *dev)
+{
+	struct v4l2_format_desc *format;
+	unsigned int i;
+	int ret;
+
+	for (i = 0; ; ++i) {
+		struct v4l2_fmtdesc fmtenum;
+
+		memset(&fmtenum, 0, sizeof fmtenum);
+		fmtenum.index = i;
+		fmtenum.type = dev->type;
+
+		ret = ioctl(dev->fd, VIDIOC_ENUM_FMT, &fmtenum);
+		if (ret < 0)
+			break;
+
+		if (i != fmtenum.index)
+			printf("Warning: driver returned wrong format index "
+				"%u.\n", fmtenum.index);
+		if (dev->type != fmtenum.type)
+			printf("Warning: driver returned wrong format type "
+				"%u.\n", fmtenum.type);
+
+		format = malloc(sizeof *format);
+		if (format == NULL)
+			return -ENOMEM;
+
+		memset(format, 0, sizeof *format);
+
+		list_init(&format->frames);
+		format->pixelformat = fmtenum.pixelformat;
+
+		list_append(&format->list, &dev->formats);
+
+		ret = v4l2_enum_frame_sizes(dev, format);
+		if (ret < 0)
+			return ret;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Open/close
+ */
+
+struct v4l2_device *v4l2_open(const char *devname)
+{
+	struct v4l2_device *dev;
+	struct v4l2_capability cap;
+	int ret;
+
+	dev = malloc(sizeof *dev);
+	if (dev == NULL)
+		return NULL;
+
+	memset(dev, 0, sizeof *dev);
+	dev->fd = -1;
+	list_init(&dev->formats);
+
+	dev->fd = open(devname, O_RDWR | O_NONBLOCK);
+	if (dev->fd < 0) {
+		printf("Error opening device %s: %d.\n", devname, errno);
+		v4l2_close(dev);
+		return NULL;
+	}
+
+	memset(&cap, 0, sizeof cap);
+	ret = ioctl(dev->fd, VIDIOC_QUERYCAP, &cap);
+	if (ret < 0) {
+		printf("Error opening device %s: unable to query "
+			"device.\n", devname);
+		v4l2_close(dev);
+		return NULL;
+	}
+
+	if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)
+		dev->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	else if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT)
+		dev->type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
+	else {
+		printf("Error opening device %s: neither video capture "
+			"nor video output supported.\n", devname);
+		v4l2_close(dev);
+		return NULL;
+	}
+
+	ret = v4l2_enum_formats(dev);
+	if (ret < 0) {
+		printf("Error opening device %s: unable to enumerate "
+			"formats.\n", devname);
+		v4l2_close(dev);
+		return NULL;
+	}
+
+	printf("Device %s opened: %s (%s).\n", devname, cap.card, cap.bus_info);
+
+	return dev;
+}
+
+void v4l2_close(struct v4l2_device *dev)
+{
+	struct v4l2_format_desc *format, *next_fmt;
+	struct v4l2_frame_desc *frame, *next_frm;
+	struct v4l2_ival_desc *ival, *next_ival;
+
+	if (dev == NULL)
+		return;
+
+	list_for_each_entry_safe(format, next_fmt, &dev->formats, list) {
+		list_for_each_entry_safe(frame, next_frm, &format->frames, list) {
+			list_for_each_entry_safe(ival, next_ival, &frame->ivals, list) {
+				free(ival);
+			}
+			free(frame);
+		}
+		free(format);
+	}
+
+	close(dev->fd);
+}
+
+/* -----------------------------------------------------------------------------
+ * Controls
+ */
+
+int v4l2_get_control(struct v4l2_device *dev, unsigned int id, int32_t *value)
+{
+	struct v4l2_control ctrl;
+	int ret;
+
+	ctrl.id = id;
+
+	ret = ioctl(dev->fd, VIDIOC_G_CTRL, &ctrl);
+	if (ret < 0) {
+		printf("unable to get control: %s (%d).\n",
+			strerror(errno), errno);
+		return -errno;
+	}
+
+	*value = ctrl.value;
+	return 0;
+}
+
+int v4l2_set_control(struct v4l2_device *dev, unsigned int id, int32_t *value)
+{
+	struct v4l2_control ctrl;
+	int ret;
+
+	ctrl.id = id;
+	ctrl.value = *value;
+
+	ret = ioctl(dev->fd, VIDIOC_S_CTRL, &ctrl);
+	if (ret < 0) {
+		printf("unable to set control: %s (%d).\n",
+			strerror(errno), errno);
+		return -errno;
+	}
+
+	*value = ctrl.value;
+	return 0;
+}
+
+int v4l2_get_controls(struct v4l2_device *dev, unsigned int count,
+		      struct v4l2_ext_control *ctrls)
+{
+	struct v4l2_ext_controls controls;
+	int ret;
+
+	memset(&controls, 0, sizeof controls);
+	controls.count = count;
+	controls.controls = ctrls;
+
+	ret = ioctl(dev->fd, VIDIOC_G_EXT_CTRLS, &controls);
+	if (ret < 0)
+		printf("unable to get multiple controls: %s (%d).\n",
+			strerror(errno), errno);
+
+	return ret;
+}
+
+int v4l2_set_controls(struct v4l2_device *dev, unsigned int count,
+		      struct v4l2_ext_control *ctrls)
+{
+	struct v4l2_ext_controls controls;
+	int ret;
+
+	memset(&controls, 0, sizeof controls);
+	controls.count = count;
+	controls.controls = ctrls;
+
+	ret = ioctl(dev->fd, VIDIOC_S_EXT_CTRLS, &controls);
+	if (ret < 0)
+		printf("unable to set multiple controls: %s (%d).\n",
+			strerror(errno), errno);
+
+	return ret;
+}
+
+/* -----------------------------------------------------------------------------
+ * Formats and frame rates
+ */
+
+int v4l2_get_crop(struct v4l2_device *dev, struct v4l2_rect *rect)
+{
+	struct v4l2_crop crop;
+	int ret;
+
+	memset(&crop, 0, sizeof crop);
+	crop.type = dev->type;
+
+	ret = ioctl(dev->fd, VIDIOC_G_CROP, &crop);
+	if (ret < 0) {
+		printf("Unable to get crop rectangle: %s (%d).\n",
+			strerror(errno), errno);
+		return -errno;
+	}
+
+	dev->crop = crop.c;
+	*rect = crop.c;
+
+	return 0;
+}
+
+int v4l2_set_crop(struct v4l2_device *dev, struct v4l2_rect *rect)
+{
+	struct v4l2_crop crop;
+	int ret;
+
+	memset(&crop, 0, sizeof crop);
+	crop.type = dev->type;
+	crop.c = *rect;
+
+	ret = ioctl(dev->fd, VIDIOC_S_CROP, &crop);
+	if (ret < 0) {
+		printf("Unable to set crop rectangle: %s (%d).\n",
+			strerror(errno), errno);
+		return -errno;
+	}
+
+	dev->crop = crop.c;
+	*rect = crop.c;
+
+	return 0;
+}
+
+int v4l2_get_format(struct v4l2_device *dev, struct v4l2_pix_format *format)
+{
+	struct v4l2_format fmt;
+	int ret;
+
+	memset(&fmt, 0, sizeof fmt);
+	fmt.type = dev->type;
+
+	ret = ioctl(dev->fd, VIDIOC_G_FMT, &fmt);
+	if (ret < 0) {
+		printf("Unable to get format: %s (%d).\n", strerror(errno),
+			errno);
+		return -errno;
+	}
+
+	dev->format = fmt.fmt.pix;
+	*format = fmt.fmt.pix;
+
+	return 0;
+}
+
+int v4l2_set_format(struct v4l2_device *dev, struct v4l2_pix_format *format)
+{
+	struct v4l2_format fmt;
+	int ret;
+
+	memset(&fmt, 0, sizeof fmt);
+	fmt.type = dev->type;
+	fmt.fmt.pix.width = format->width;
+	fmt.fmt.pix.height = format->height;
+	fmt.fmt.pix.pixelformat = format->pixelformat;
+	fmt.fmt.pix.field = V4L2_FIELD_ANY;
+
+	ret = ioctl(dev->fd, VIDIOC_S_FMT, &fmt);
+	if (ret < 0) {
+		printf("Unable to set format: %s (%d).\n", strerror(errno),
+			errno);
+		return -errno;
+	}
+
+	dev->format = fmt.fmt.pix;
+	*format = fmt.fmt.pix;
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Buffers management
+ */
+
+int v4l2_alloc_buffers(struct v4l2_device *dev, struct v4l2_buffers_pool *pool,
+		       enum v4l2_memory memtype)
+{
+	struct v4l2_requestbuffers rb;
+	struct v4l2_buffer buf;
+	unsigned int i;
+	int ret;
+
+	dev->memtype = memtype;
+	dev->pool = pool;
+
+	memset(&rb, 0, sizeof rb);
+	rb.count = pool->nbufs;
+	rb.type = dev->type;
+	rb.memory = memtype;
+
+	ret = ioctl(dev->fd, VIDIOC_REQBUFS, &rb);
+	if (ret < 0) {
+		printf("Unable to request buffers: %d.\n", errno);
+		ret = -errno;
+		goto done;
+	}
+
+	if (rb.count > pool->nbufs) {
+		printf("Driver needs more buffers (%u) than available (%u).\n",
+		       rb.count, pool->nbufs);
+		ret = -E2BIG;
+		goto done;
+	}
+
+	dev->nbufs = rb.count;
+
+	printf("%u buffers requested.\n", rb.count);
+
+	/* Map the buffers. */
+	for (i = 0; i < rb.count; ++i) {
+		memset(&buf, 0, sizeof buf);
+		buf.index = i;
+		buf.type = dev->type;
+		buf.memory = memtype;
+		ret = ioctl(dev->fd, VIDIOC_QUERYBUF, &buf);
+		if (ret < 0) {
+			printf("Unable to query buffer %u (%d).\n", i, errno);
+			ret = -errno;
+			goto done;
+		}
+
+		switch (memtype) {
+		case V4L2_MEMORY_MMAP:
+			pool->buffers[i].mem = mmap(0, buf.length, PROT_READ | PROT_WRITE,
+					      MAP_SHARED, dev->fd, buf.m.offset);
+			if (pool->buffers[i].mem == MAP_FAILED) {
+				printf("Unable to map buffer %u (%d)\n", i, errno);
+				ret = -errno;
+				goto done;
+			}
+			pool->buffers[i].size = buf.length;
+			printf("Buffer %u mapped at address %p.\n", i, pool->buffers[i].mem);
+			break;
+
+		case V4L2_MEMORY_USERPTR:
+			if (pool->buffers[i].size < buf.length) {
+				printf("Buffer %u too small (%u bytes required, "
+				       "%u bytes available.\n", i, buf.length,
+				       pool->buffers[i].size);
+				ret = -EINVAL;
+				goto done;
+			}
+
+			printf("Buffer %u valid.\n", i);
+			break;
+
+		default:
+			break;
+		}
+	}
+
+	ret = 0;
+
+done:
+	if (ret < 0)
+		v4l2_free_buffers(dev);
+
+	return ret;
+}
+
+int v4l2_free_buffers(struct v4l2_device *dev)
+{
+	struct v4l2_requestbuffers rb;
+	unsigned int i;
+	int ret;
+
+	if (dev->pool == NULL)
+		return 0;
+
+	if (dev->memtype == V4L2_MEMORY_MMAP) {
+		for (i = 0; i < dev->nbufs; ++i) {
+			struct v4l2_video_buffer *buffer = &dev->pool->buffers[i];
+
+			if (buffer->mem == NULL)
+				continue;
+
+			ret = munmap(buffer->mem, buffer->size);
+			if (ret < 0) {
+				printf("Unable to unmap buffer %u (%d)\n", i, errno);
+				return -errno;
+			}
+
+			buffer->mem = NULL;
+			buffer->size = 0;
+		}
+	}
+
+	memset(&rb, 0, sizeof rb);
+	rb.count = 0;
+	rb.type = dev->type;
+	rb.memory = dev->memtype;
+
+	ret = ioctl(dev->fd, VIDIOC_REQBUFS, &rb);
+	if (ret < 0) {
+		printf("Unable to release buffers: %d.\n", errno);
+		return -errno;
+	}
+
+	dev->nbufs = 0;
+	dev->pool = NULL;
+
+	return 0;
+}
+
+int v4l2_dequeue_buffer(struct v4l2_device *dev, struct v4l2_video_buffer *buffer)
+{
+	struct v4l2_buffer buf;
+	int ret;
+
+	memset(&buf, 0, sizeof buf);
+	buf.type = dev->type;
+	buf.memory = dev->memtype;
+
+	ret = ioctl(dev->fd, VIDIOC_DQBUF, &buf);
+	if (ret < 0) {
+		printf("Unable to dequeue buffer (%d).\n", errno);
+		return -errno;
+	}
+
+	*buffer = dev->pool->buffers[buf.index];
+	buffer->index = buf.index;
+	buffer->bytesused = buf.bytesused;
+	buffer->timestamp = buf.timestamp;
+	buffer->error = !!(buf.flags & V4L2_BUF_FLAG_ERROR);
+
+	return 0;
+}
+
+int v4l2_queue_buffer(struct v4l2_device *dev, struct v4l2_video_buffer *buffer)
+{
+	struct v4l2_buffer buf;
+	int ret;
+
+	if (buffer->index >= dev->nbufs)
+		return -EINVAL;
+
+	memset(&buf, 0, sizeof buf);
+	buf.index = buffer->index;
+	buf.type = dev->type;
+	buf.memory = dev->memtype;
+
+	if (dev->memtype == V4L2_MEMORY_USERPTR)
+		buf.m.userptr = (unsigned long)dev->pool->buffers[buffer->index].mem;
+
+	buf.length = dev->pool->buffers[buffer->index].size;
+
+	if (dev->type == V4L2_BUF_TYPE_VIDEO_OUTPUT)
+		buf.bytesused = buffer->bytesused;
+
+	ret = ioctl(dev->fd, VIDIOC_QBUF, &buf);
+	if (ret < 0) {
+		printf("Unable to queue buffer (%d).\n", errno);
+		return -errno;
+	}
+
+	return 0;
+}
+
+/* -----------------------------------------------------------------------------
+ * Stream management
+ */
+
+int v4l2_stream_on(struct v4l2_device *dev)
+{
+	int type = dev->type;
+	int ret;
+
+	ret = ioctl(dev->fd, VIDIOC_STREAMON, &type);
+	if (ret < 0)
+		return -errno;
+
+	return 0;
+}
+
+int v4l2_stream_off(struct v4l2_device *dev)
+{
+	int type = dev->type;
+	int ret;
+
+	ret = ioctl(dev->fd, VIDIOC_STREAMOFF, &type);
+	if (ret < 0)
+		return -errno;
+
+	return 0;
+}