summaryrefslogtreecommitdiff
path: root/v4l2-apps/libv4l/libv4l2/libv4l2.c
diff options
context:
space:
mode:
Diffstat (limited to 'v4l2-apps/libv4l/libv4l2/libv4l2.c')
-rw-r--r--v4l2-apps/libv4l/libv4l2/libv4l2.c1145
1 files changed, 1145 insertions, 0 deletions
diff --git a/v4l2-apps/libv4l/libv4l2/libv4l2.c b/v4l2-apps/libv4l/libv4l2/libv4l2.c
new file mode 100644
index 000000000..b4a10afac
--- /dev/null
+++ b/v4l2-apps/libv4l/libv4l2/libv4l2.c
@@ -0,0 +1,1145 @@
+/*
+# (C) 2008 Hans de Goede <j.w.r.degoede@hhs.nl>
+
+# This program 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 program 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
+# Lesser General Public License for more details.
+#
+# You should have received a copy of the GNU Lesser General Public License
+# along with this program; if not, write to the Free Software
+# Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+*/
+
+/* MAKING CHANGES TO THIS FILE?? READ THIS FIRST!!!
+
+ This file implements libv4l2, which offers v4l2_ prefixed versions of
+ open/close/etc. The API is 100% the same as directly opening /dev/videoX
+ using regular open/close/etc, the big difference is that format conversion
+ is done if necessary when capturing. That is if you (try to) set a capture
+ format which is not supported by the cam, but is supported by libv4lconvert,
+ then the try_fmt / set_fmt will succeed as if the cam supports the format
+ and on dqbuf / read the data will be converted for you and returned in
+ the request format.
+
+ Important note to people making changes to this file: All functions
+ (v4l2_close, v4l2_ioctl, etc.) are designed to function as their regular
+ counterpart when they get passed a fd that is not "registered" by libv4l2,
+ there are 2 reasons for this:
+ 1) This allows us to get completely out of the way when dealing with non
+ capture devices.
+ 2) libv4l2 is the base of the v4l2convert.so wrapper lib, which is a .so
+ which can be LD_PRELOAD-ed and the overrules the libc's open/close/etc,
+ and when opening /dev/videoX or /dev/v4l/ calls v4l2_open. Because we
+ behave as the regular counterpart when the fd is not known (instead of say
+ throwing an error), v4l2convert.so can simply call the v4l2_ prefixed
+ function for all wrapped functions (except for v4l2_open which will fail
+ when not called on a v4l2 device). This way the wrapper does not have to
+ keep track of which fd's are being handled by libv4l2, as libv4l2 already
+ keeps track of this itself.
+
+ This also means that libv4l2 may not use any of the regular functions
+ it mimics, as for example open could be a symbol in v4l2convert.so, which
+ in turn will call v4l2_open, so therefor v4l2_open (for example) may not
+ use the regular open()!
+
+ Another important note: libv4l2 does conversion for capture usage only, if
+ any calls are made which are passed a v4l2_buffer or v4l2_format with a
+ v4l2_buf_type which is different from V4L2_BUF_TYPE_VIDEO_CAPTURE, then
+ the v4l2_ methods behave exactly the same as their regular counterparts.
+ When modifications are made, one should be carefull that this behavior is
+ preserved.
+*/
+#include <errno.h>
+#include <stdarg.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <syscall.h>
+#include <fcntl.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include "libv4l2.h"
+#include "libv4l2-priv.h"
+
+/* Note these flags are stored together with the flags passed to v4l2_fd_open()
+ in v4l2_dev_info's flags member, so care should be taken that the do not
+ use the same bits! */
+#define V4L2_STREAMON 0x0100
+#define V4L2_BUFFERS_REQUESTED_BY_READ 0x0200
+#define V4L2_STREAM_CONTROLLED_BY_READ 0x0400
+#define V4L2_SUPPORTS_READ 0x0800
+
+#define V4L2_MMAP_OFFSET_MAGIC 0xABCDEF00u
+
+static pthread_mutex_t v4l2_open_mutex = PTHREAD_MUTEX_INITIALIZER;
+static struct v4l2_dev_info devices[V4L2_MAX_DEVICES] = { { .fd = -1 },
+ { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 },
+ { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 },
+ { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }, { .fd = -1 }};
+static int devices_used = 0;
+
+
+static int v4l2_request_read_buffers(int index)
+{
+ int result;
+ struct v4l2_requestbuffers req;
+
+ /* Note we re-request the buffers if they are already requested as the format
+ and thus the needed buffersize may have changed. */
+ req.count = (devices[index].no_frames)? devices[index].no_frames:
+ devices[index].nreadbuffers;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+ if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_REQBUFS, &req)) < 0){
+ int saved_err = errno;
+ V4L2_LOG_ERR("requesting %u buffers: %s\n", req.count, strerror(errno));
+ errno = saved_err;
+ return result;
+ }
+
+ if (!devices[index].no_frames && req.count)
+ devices[index].flags |= V4L2_BUFFERS_REQUESTED_BY_READ;
+
+ devices[index].no_frames = MIN(req.count, V4L2_MAX_NO_FRAMES);
+ return 0;
+}
+
+static void v4l2_unrequest_read_buffers(int index)
+{
+ struct v4l2_requestbuffers req;
+
+ if (!(devices[index].flags & V4L2_BUFFERS_REQUESTED_BY_READ) ||
+ devices[index].no_frames == 0)
+ return;
+
+ /* (Un)Request buffers, note not all driver support this, and those
+ who do not support it don't need it. */
+ req.count = 0;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+ if(syscall(SYS_ioctl, devices[index].fd, VIDIOC_REQBUFS, &req) < 0)
+ return;
+
+ devices[index].no_frames = MIN(req.count, V4L2_MAX_NO_FRAMES);
+ if (devices[index].no_frames == 0)
+ devices[index].flags &= ~V4L2_BUFFERS_REQUESTED_BY_READ;
+}
+
+static int v4l2_map_buffers(int index)
+{
+ int result = 0;
+ unsigned int i;
+ struct v4l2_buffer buf;
+
+ for (i = 0; i < devices[index].no_frames; i++) {
+ if (devices[index].frame_pointers[i] != MAP_FAILED)
+ continue;
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYBUF, &buf);
+ if (result) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("querying buffer %u: %s\n", i, strerror(errno));
+ errno = saved_err;
+ break;
+ }
+
+ devices[index].frame_pointers[i] = (void *)syscall(SYS_mmap2, NULL,
+ (size_t)buf.length, PROT_READ|PROT_WRITE, MAP_SHARED, devices[index].fd,
+ (__off_t)(buf.m.offset >> MMAP2_PAGE_SHIFT));
+ if (devices[index].frame_pointers[i] == MAP_FAILED) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("mmapping buffer %u: %s\n", i, strerror(errno));
+ errno = saved_err;
+ result = -1;
+ break;
+ }
+ V4L2_LOG("mapped buffer %u at %p\n", i,
+ devices[index].frame_pointers[i]);
+
+ devices[index].frame_sizes[i] = buf.length;
+ }
+
+ return result;
+}
+
+static void v4l2_unmap_buffers(int index)
+{
+ unsigned int i;
+
+ /* unmap the buffers */
+ for (i = 0; i < devices[index].no_frames; i++) {
+ if (devices[index].frame_pointers[i] != MAP_FAILED) {
+ syscall(SYS_munmap, devices[index].frame_pointers[i],
+ devices[index].frame_sizes[i]);
+ devices[index].frame_pointers[i] = MAP_FAILED;
+ V4L2_LOG("unmapped buffer %u\n", i);
+ }
+ }
+}
+
+static int v4l2_streamon(int index)
+{
+ int result;
+ enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (!(devices[index].flags & V4L2_STREAMON)) {
+ if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_STREAMON,
+ &type))) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("turning on stream: %s\n", strerror(errno));
+ errno = saved_err;
+ return result;
+ }
+ devices[index].flags |= V4L2_STREAMON;
+ }
+
+ return 0;
+}
+
+static int v4l2_streamoff(int index)
+{
+ int result;
+ enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (devices[index].flags & V4L2_STREAMON) {
+ if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_STREAMOFF,
+ &type))) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("turning off stream: %s\n", strerror(errno));
+ errno = saved_err;
+ return result;
+ }
+ devices[index].flags &= ~V4L2_STREAMON;
+
+ /* Stream off also unqueues all our buffers! */
+ devices[index].frame_queued = 0;
+ }
+
+ return 0;
+}
+
+static int v4l2_queue_read_buffer(int index, int buffer_index)
+{
+ int result;
+ struct v4l2_buffer buf;
+
+ if (devices[index].frame_queued & (1 << buffer_index))
+ return 0;
+
+ memset(&buf, 0, sizeof(buf));
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = buffer_index;
+ if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QBUF, &buf))) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("queuing buf %d: %s\n", buffer_index, strerror(errno));
+ errno = saved_err;
+ return result;
+ }
+
+ devices[index].frame_queued |= 1 << buffer_index;
+ return 0;
+}
+
+static int v4l2_dequeue_and_convert(int index, struct v4l2_buffer *buf,
+ unsigned char *dest, int dest_size)
+{
+ const int max_tries = 10;
+ int result, tries = max_tries;
+
+ /* Make sure we have the real v4l2 buffers mapped */
+ if ((result = v4l2_map_buffers(index)))
+ return result;
+
+ do {
+ if ((result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_DQBUF, buf))) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("dequeuing buf: %s\n", strerror(errno));
+ errno = saved_err;
+ return result;
+ }
+
+ devices[index].frame_queued &= ~(1 << buf->index);
+
+ result = v4lconvert_convert(devices[index].convert,
+ &devices[index].src_fmt, &devices[index].dest_fmt,
+ devices[index].frame_pointers[buf->index],
+ buf->bytesused, dest ? dest : (devices[index].convert_mmap_buf +
+ buf->index * V4L2_FRAME_BUF_SIZE), dest_size);
+
+ if (result < 0) {
+ int saved_err = errno;
+
+ if(errno == EAGAIN)
+ V4L2_LOG("warning error while converting frame data: %s\n",
+ v4lconvert_get_error_message(devices[index].convert));
+ else
+ V4L2_LOG_ERR("converting / decoding frame data: %s\n",
+ v4lconvert_get_error_message(devices[index].convert));
+
+ v4l2_queue_read_buffer(index, buf->index);
+ errno = saved_err;
+ }
+ tries--;
+ } while (result < 0 && errno == EAGAIN && tries);
+
+ if (result < 0 && errno == EAGAIN) {
+ V4L2_LOG_ERR("got %d consecutive frame decode errors, last error: %s\n",
+ max_tries, v4lconvert_get_error_message(devices[index].convert));
+ }
+
+ return result;
+}
+
+static int v4l2_queue_read_buffers(int index)
+{
+ unsigned int i;
+ int last_error = EIO, queued = 0;
+
+ for (i = 0; i < devices[index].no_frames; i++) {
+ /* Don't queue unmapped buffers (should never happen) */
+ if (devices[index].frame_pointers[i] != MAP_FAILED) {
+ if (v4l2_queue_read_buffer(index, i)) {
+ last_error = errno;
+ continue;
+ }
+ queued++;
+ }
+ }
+
+ if (!queued) {
+ errno = last_error;
+ return -1;
+ }
+ return 0;
+}
+
+static int v4l2_activate_read_stream(int index)
+{
+ int result;
+
+ if ((result = v4l2_request_read_buffers(index)))
+ return result;
+
+ if ((result = v4l2_map_buffers(index)))
+ return result;
+
+ if ((result = v4l2_queue_read_buffers(index)))
+ return result;
+
+ devices[index].flags |= V4L2_STREAM_CONTROLLED_BY_READ;
+
+ return result = v4l2_streamon(index);
+}
+
+static int v4l2_deactivate_read_stream(int index)
+{
+ int result;
+
+ if ((result = v4l2_streamoff(index)))
+ return result;
+
+ /* No need to unqueue our buffers, streamoff does that for us */
+
+ v4l2_unmap_buffers(index);
+
+ v4l2_unrequest_read_buffers(index);
+
+ devices[index].flags &= ~V4L2_STREAM_CONTROLLED_BY_READ;
+
+ return 0;
+}
+
+static int v4l2_buffers_mapped(int index)
+{
+ unsigned int i;
+
+ if (devices[index].src_fmt.fmt.pix.pixelformat ==
+ devices[index].dest_fmt.fmt.pix.pixelformat) {
+ /* Normal (no conversion) mode */
+ struct v4l2_buffer buf;
+
+ for (i = 0; i < devices[index].no_frames; i++) {
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+ if (syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYBUF, &buf)) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("querying buffer %u: %s\n", i, strerror(errno));
+ errno = saved_err;
+ break;
+ }
+ if (buf.flags & V4L2_BUF_FLAG_MAPPED)
+ break;
+ }
+ } else {
+ /* Conversion mode */
+ for (i = 0; i < devices[index].no_frames; i++)
+ if (devices[index].frame_map_count[i])
+ break;
+ }
+
+ if (i != devices[index].no_frames)
+ V4L2_LOG("v4l2_buffers_mapped(): buffers still mapped\n");
+
+ return i != devices[index].no_frames;
+}
+
+
+int v4l2_open (const char *file, int oflag, ...)
+{
+ int fd;
+
+ /* original open code */
+ if (oflag & O_CREAT)
+ {
+ va_list ap;
+ mode_t mode;
+
+ va_start (ap, oflag);
+ mode = va_arg (ap, mode_t);
+
+ fd = syscall(SYS_open, file, oflag, mode);
+
+ va_end(ap);
+ }
+ else
+ fd = syscall(SYS_open, file, oflag);
+ /* end of original open code */
+
+ if (fd == -1)
+ return fd;
+
+ if (v4l2_fd_open(fd, 0) == -1) {
+ int saved_err = errno;
+ syscall(SYS_close, fd);
+ errno = saved_err;
+ return -1;
+ }
+
+ return fd;
+}
+
+int v4l2_fd_open(int fd, int v4l2_flags)
+{
+ int i, index;
+ char *lfname;
+ struct v4l2_capability cap;
+ struct v4l2_format fmt;
+ struct v4lconvert_data *convert;
+
+ /* If no log file was set by the app, see if one was specified through the
+ environment */
+ if (!v4l2_log_file && (lfname = getenv("LIBV4L2_LOG_FILENAME")))
+ v4l2_log_file = fopen(lfname, "w");
+
+ /* check that this is an v4l2 device */
+ if (syscall(SYS_ioctl, fd, VIDIOC_QUERYCAP, &cap)) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("getting capabilities: %s\n", strerror(errno));
+ errno = saved_err;
+ return -1;
+ }
+
+ /* we only add functionality for video capture devices, and we do not
+ handle devices which don't do mmap */
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) ||
+ !(cap.capabilities & V4L2_CAP_STREAMING))
+ return fd;
+
+ /* Get current cam format */
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ if (syscall(SYS_ioctl, fd, VIDIOC_G_FMT, &fmt)) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("getting pixformat: %s\n", strerror(errno));
+ errno = saved_err;
+ return -1;
+ }
+
+ /* init libv4lconvert */
+ if (!(convert = v4lconvert_create(fd)))
+ return -1;
+
+ /* So we have a v4l2 capture device, register it in our devices array */
+ pthread_mutex_lock(&v4l2_open_mutex);
+ for (index = 0; index < V4L2_MAX_DEVICES; index++)
+ if(devices[index].fd == -1) {
+ devices[index].fd = fd;
+ break;
+ }
+ pthread_mutex_unlock(&v4l2_open_mutex);
+
+ if (index == V4L2_MAX_DEVICES) {
+ V4L2_LOG_ERR("attempting to open more then %d video devices\n",
+ V4L2_MAX_DEVICES);
+ errno = EBUSY;
+ return -1;
+ }
+
+ devices[index].flags = v4l2_flags;
+ if (cap.capabilities & V4L2_CAP_READWRITE)
+ devices[index].flags |= V4L2_SUPPORTS_READ;
+ devices[index].open_count = 1;
+ devices[index].src_fmt = fmt;
+ devices[index].dest_fmt = fmt;
+
+ pthread_mutex_init(&devices[index].stream_lock, NULL);
+
+ devices[index].no_frames = 0;
+ devices[index].nreadbuffers = V4L2_DEFAULT_NREADBUFFERS;
+ devices[index].convert = convert;
+ devices[index].convert_mmap_buf = MAP_FAILED;
+ for (i = 0; i < V4L2_MAX_NO_FRAMES; i++) {
+ devices[index].frame_pointers[i] = MAP_FAILED;
+ devices[index].frame_map_count[i] = 0;
+ }
+ devices[index].frame_queued = 0;
+
+ if (index >= devices_used)
+ devices_used = index + 1;
+
+ V4L2_LOG("open: %d\n", fd);
+
+ return fd;
+}
+
+/* Is this an fd for which we are emulating v4l1 ? */
+static int v4l2_get_index(int fd)
+{
+ int index;
+
+ /* We never handle fd -1 */
+ if (fd == -1)
+ return -1;
+
+ for (index = 0; index < devices_used; index++)
+ if (devices[index].fd == fd)
+ break;
+
+ if (index == devices_used)
+ return -1;
+
+ return index;
+}
+
+
+int v4l2_close(int fd)
+{
+ int index, result;
+
+ if ((index = v4l2_get_index(fd)) == -1)
+ return syscall(SYS_close, fd);
+
+ /* Abuse stream_lock to stop 2 closes from racing and trying to free the
+ resources twice */
+ pthread_mutex_lock(&devices[index].stream_lock);
+ devices[index].open_count--;
+ result = devices[index].open_count != 0;
+ pthread_mutex_unlock(&devices[index].stream_lock);
+
+ if (result)
+ return 0;
+
+ /* Free resources */
+ v4l2_unmap_buffers(index);
+ v4lconvert_destroy(devices[index].convert);
+ if (devices[index].convert_mmap_buf != MAP_FAILED) {
+ if (v4l2_buffers_mapped(index))
+ V4L2_LOG_WARN("v4l2 mmap buffers still mapped on close()\n");
+ else
+ syscall(SYS_munmap, devices[index].convert_mmap_buf,
+ devices[index].no_frames * V4L2_FRAME_BUF_SIZE);
+ devices[index].convert_mmap_buf = MAP_FAILED;
+ }
+
+ /* Remove the fd from our list of managed fds before closing it, because as
+ soon as we've done the actual close the fd maybe returned by an open in
+ another thread and we don't want to intercept calls to this new fd. */
+ devices[index].fd = -1;
+
+ /* Since we've marked the fd as no longer used, and freed the resources,
+ redo the close in case it was interrupted */
+ do {
+ result = syscall(SYS_close, fd);
+ } while (result == -1 && errno == EINTR);
+
+ V4L2_LOG("close: %d\n", fd);
+
+ return result;
+}
+
+int v4l2_dup(int fd)
+{
+ int index;
+
+ if ((index = v4l2_get_index(fd)) == -1)
+ return syscall(SYS_dup, fd);
+
+ devices[index].open_count++;
+
+ return fd;
+}
+
+static int v4l2_check_buffer_change_ok(int index)
+{
+ v4l2_unmap_buffers(index);
+
+ /* Check if the app itself still is using the stream */
+ if (v4l2_buffers_mapped(index) ||
+ (!(devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ) &&
+ ((devices[index].flags & V4L2_STREAMON) ||
+ devices[index].frame_queued))) {
+ V4L2_LOG("v4l2_check_buffer_change_ok(): stream busy\n");
+ errno = EBUSY;
+ return -1;
+ }
+
+ /* We may change from convert to non conversion mode and
+ v4l2_unrequest_read_buffers may change the no_frames, so free the
+ convert mmap buffer */
+ syscall(SYS_munmap, devices[index].convert_mmap_buf,
+ devices[index].no_frames * V4L2_FRAME_BUF_SIZE);
+ devices[index].convert_mmap_buf = MAP_FAILED;
+
+ if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ) {
+ V4L2_LOG("deactivating read-stream for settings change\n");
+ return v4l2_deactivate_read_stream(index);
+ }
+
+ return 0;
+}
+
+int v4l2_ioctl (int fd, unsigned long int request, ...)
+{
+ void *arg;
+ va_list ap;
+ int result, converting, index, saved_err;
+ int is_capture_request = 0, stream_needs_locking = 0;
+
+ va_start (ap, request);
+ arg = va_arg (ap, void *);
+ va_end (ap);
+
+ if ((index = v4l2_get_index(fd)) == -1)
+ return syscall(SYS_ioctl, fd, request, arg);
+
+ /* Appearantly the kernel and / or glibc ignore the 32 most significant bits
+ when long = 64 bits, and some applications pass an int holding the req to
+ ioctl, causing it to get sign extended, depending upon this behavior */
+ request = (unsigned int)request;
+
+ /* Is this a capture request and do we need to take the stream lock? */
+ switch (request) {
+ case VIDIOC_QUERYCAP:
+ is_capture_request = 1;
+ break;
+ case VIDIOC_ENUM_FMT:
+ if (((struct v4l2_fmtdesc *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE &&
+ (devices[index].flags & V4L2_ENABLE_ENUM_FMT_EMULATION))
+ is_capture_request = 1;
+ break;
+ case VIDIOC_ENUM_FRAMESIZES:
+ case VIDIOC_ENUM_FRAMEINTERVALS:
+ if (devices[index].flags & V4L2_ENABLE_ENUM_FMT_EMULATION)
+ is_capture_request = 1;
+ break;
+ case VIDIOC_TRY_FMT:
+ if (((struct v4l2_format *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE)
+ is_capture_request = 1;
+ break;
+ case VIDIOC_S_FMT:
+ case VIDIOC_G_FMT:
+ if (((struct v4l2_format *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+ is_capture_request = 1;
+ stream_needs_locking = 1;
+ }
+ break;
+ case VIDIOC_REQBUFS:
+ if (((struct v4l2_requestbuffers *)arg)->type ==
+ V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+ is_capture_request = 1;
+ stream_needs_locking = 1;
+ }
+ break;
+ case VIDIOC_QUERYBUF:
+ case VIDIOC_QBUF:
+ case VIDIOC_DQBUF:
+ if (((struct v4l2_buffer *)arg)->type == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+ is_capture_request = 1;
+ stream_needs_locking = 1;
+ }
+ break;
+ case VIDIOC_STREAMON:
+ case VIDIOC_STREAMOFF:
+ if (*((enum v4l2_buf_type *)arg) == V4L2_BUF_TYPE_VIDEO_CAPTURE) {
+ is_capture_request = 1;
+ stream_needs_locking = 1;
+ }
+ }
+
+ if (!is_capture_request) {
+ result = syscall(SYS_ioctl, fd, request, arg);
+ saved_err = errno;
+ v4l2_log_ioctl(request, arg, result);
+ errno = saved_err;
+ return result;
+ }
+
+
+ if (stream_needs_locking)
+ pthread_mutex_lock(&devices[index].stream_lock);
+
+ converting = v4lconvert_needs_conversion(devices[index].convert,
+ &devices[index].src_fmt, &devices[index].dest_fmt);
+
+ switch (request) {
+ case VIDIOC_QUERYCAP:
+ {
+ struct v4l2_capability *cap = arg;
+
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYCAP, cap);
+ if (result == 0)
+ /* We always support read() as we fake it using mmap mode */
+ cap->capabilities |= V4L2_CAP_READWRITE;
+ }
+ break;
+
+ case VIDIOC_ENUM_FMT:
+ result = v4lconvert_enum_fmt(devices[index].convert, arg);
+ break;
+
+ case VIDIOC_ENUM_FRAMESIZES:
+ result = v4lconvert_enum_framesizes(devices[index].convert, arg);
+ break;
+
+ case VIDIOC_ENUM_FRAMEINTERVALS:
+ result = v4lconvert_enum_frameintervals(devices[index].convert, arg);
+ break;
+
+ case VIDIOC_TRY_FMT:
+ result = v4lconvert_try_format(devices[index].convert, arg, NULL);
+ break;
+
+ case VIDIOC_S_FMT:
+ {
+ struct v4l2_format src_fmt, *dest_fmt = arg;
+
+ if (!memcmp(&devices[index].dest_fmt, dest_fmt, sizeof(*dest_fmt))) {
+ result = 0;
+ break;
+ }
+
+ if (devices[index].flags & V4L2_DISABLE_CONVERSION) {
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_TRY_FMT,
+ dest_fmt);
+ src_fmt = *dest_fmt;
+ } else {
+ result = v4lconvert_try_format(devices[index].convert, dest_fmt,
+ &src_fmt);
+ }
+
+ if (result)
+ break;
+
+ if (src_fmt.fmt.pix.pixelformat != dest_fmt->fmt.pix.pixelformat &&
+ v4l2_log_file) {
+ int pixfmt = src_fmt.fmt.pix.pixelformat;
+
+ fprintf(v4l2_log_file, "VIDIOC_S_FMT converting from: %c%c%c%c\n",
+ pixfmt & 0xff,
+ (pixfmt >> 8) & 0xff,
+ (pixfmt >> 16) & 0xff,
+ pixfmt >> 24);
+ }
+
+ /* Maybe after try format has adjusted width/height etc, to whats
+ available nothing has changed (on the cam side) ? */
+ if (!memcmp(&devices[index].src_fmt, &src_fmt, sizeof(src_fmt))) {
+ devices[index].dest_fmt = *dest_fmt;
+ result = 0;
+ break;
+ }
+
+ if ((result = v4l2_check_buffer_change_ok(index)))
+ break;
+
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_S_FMT, &src_fmt);
+ if (result) {
+ saved_err = errno;
+ V4L2_LOG_ERR("setting pixformat: %s\n", strerror(errno));
+ /* Report to the app dest_fmt has not changed */
+ *dest_fmt = devices[index].dest_fmt;
+ errno = saved_err;
+ break;
+ }
+
+ devices[index].src_fmt = src_fmt;
+ devices[index].dest_fmt = *dest_fmt;
+ }
+ break;
+
+ case VIDIOC_G_FMT:
+ {
+ struct v4l2_format* fmt = arg;
+
+ *fmt = devices[index].dest_fmt;
+ result = 0;
+ }
+ break;
+
+ case VIDIOC_REQBUFS:
+ {
+ struct v4l2_requestbuffers *req = arg;
+
+ /* IMPROVEME (maybe?) add support for userptr's? */
+ if (req->memory != V4L2_MEMORY_MMAP) {
+ errno = EINVAL;
+ result = -1;
+ break;
+ }
+
+ if ((result = v4l2_check_buffer_change_ok(index)))
+ break;
+
+ /* No more buffers then we can manage please */
+ if (req->count > V4L2_MAX_NO_FRAMES)
+ req->count = V4L2_MAX_NO_FRAMES;
+
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_REQBUFS, req);
+ if (result < 0)
+ break;
+ result = 0; /* some drivers return the number of buffers on success */
+
+ devices[index].no_frames = MIN(req->count, V4L2_MAX_NO_FRAMES);
+ devices[index].flags &= ~V4L2_BUFFERS_REQUESTED_BY_READ;
+ }
+ break;
+
+ case VIDIOC_QUERYBUF:
+ {
+ struct v4l2_buffer *buf = arg;
+
+ if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
+ if ((result = v4l2_deactivate_read_stream(index)))
+ break;
+
+ /* Do a real query even when converting to let the driver fill in
+ things like buf->field */
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QUERYBUF, buf);
+ if (result || !converting)
+ break;
+
+ buf->m.offset = V4L2_MMAP_OFFSET_MAGIC | buf->index;
+ buf->length = V4L2_FRAME_BUF_SIZE;
+ if (devices[index].frame_map_count[buf->index])
+ buf->flags |= V4L2_BUF_FLAG_MAPPED;
+ else
+ buf->flags &= ~V4L2_BUF_FLAG_MAPPED;
+ }
+ break;
+
+ case VIDIOC_QBUF:
+ if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
+ if ((result = v4l2_deactivate_read_stream(index)))
+ break;
+
+ /* With some drivers the buffers must be mapped before queuing */
+ if (converting)
+ if ((result = v4l2_map_buffers(index)))
+ break;
+
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_QBUF, arg);
+ break;
+
+ case VIDIOC_DQBUF:
+ {
+ struct v4l2_buffer *buf = arg;
+
+ if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
+ if ((result = v4l2_deactivate_read_stream(index)))
+ break;
+
+ if (!converting) {
+ result = syscall(SYS_ioctl, devices[index].fd, VIDIOC_DQBUF, buf);
+ if (result) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("dequeuing buf: %s\n", strerror(errno));
+ errno = saved_err;
+ }
+ break;
+ }
+
+ /* An application can do a DQBUF before mmap-ing in the buffer,
+ but we need the buffer _now_ to write our converted data
+ to it! */
+ if (devices[index].convert_mmap_buf == MAP_FAILED) {
+ devices[index].convert_mmap_buf = (void *)syscall(SYS_mmap2,
+ (size_t)(
+ devices[index].no_frames *
+ V4L2_FRAME_BUF_SIZE),
+ PROT_READ|PROT_WRITE,
+ MAP_ANONYMOUS|MAP_PRIVATE,
+ -1, 0);
+ if (devices[index].convert_mmap_buf == MAP_FAILED) {
+ saved_err = errno;
+ V4L2_LOG_ERR("allocating conversion buffer\n");
+ errno = saved_err;
+ result = -1;
+ break;
+ }
+ }
+
+ result = v4l2_dequeue_and_convert(index, buf, 0, V4L2_FRAME_BUF_SIZE);
+ if (result < 0)
+ break;
+
+ buf->bytesused = result;
+ buf->m.offset = V4L2_MMAP_OFFSET_MAGIC | buf->index;
+ buf->length = V4L2_FRAME_BUF_SIZE;
+ if (devices[index].frame_map_count[buf->index])
+ buf->flags |= V4L2_BUF_FLAG_MAPPED;
+ else
+ buf->flags &= ~V4L2_BUF_FLAG_MAPPED;
+
+ result = 0;
+ }
+ break;
+
+ case VIDIOC_STREAMON:
+ case VIDIOC_STREAMOFF:
+ if (devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)
+ if ((result = v4l2_deactivate_read_stream(index)))
+ break;
+
+ if (request == VIDIOC_STREAMON)
+ result = v4l2_streamon(index);
+ else
+ result = v4l2_streamoff(index);
+ break;
+
+ default:
+ result = syscall(SYS_ioctl, fd, request, arg);
+ }
+
+ if (stream_needs_locking)
+ pthread_mutex_unlock(&devices[index].stream_lock);
+
+ saved_err = errno;
+ v4l2_log_ioctl(request, arg, result);
+ errno = saved_err;
+
+ return result;
+}
+
+
+ssize_t v4l2_read (int fd, void* dest, size_t n)
+{
+ ssize_t result;
+ int index;
+ struct v4l2_buffer buf;
+
+ if ((index = v4l2_get_index(fd)) == -1)
+ return syscall(SYS_read, fd, dest, n);
+
+ pthread_mutex_lock(&devices[index].stream_lock);
+
+ /* When not converting and the device supports read let the kernel handle
+ it */
+ if ((devices[index].flags & V4L2_SUPPORTS_READ) &&
+ !v4lconvert_needs_conversion(devices[index].convert,
+ &devices[index].src_fmt, &devices[index].dest_fmt)) {
+ result = syscall(SYS_read, fd, dest, n);
+ goto leave;
+ }
+
+ if (!(devices[index].flags & V4L2_STREAM_CONTROLLED_BY_READ)) {
+ if ((devices[index].flags & V4L2_STREAMON) ||
+ devices[index].frame_queued) {
+ errno = EBUSY;
+ result = -1;
+ goto leave;
+ }
+ if ((result = v4l2_activate_read_stream(index)))
+ goto leave;
+ }
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ result = v4l2_dequeue_and_convert(index, &buf, dest, n);
+
+ if (result >= 0)
+ v4l2_queue_read_buffer(index, buf.index);
+
+leave:
+ pthread_mutex_unlock(&devices[index].stream_lock);
+
+ return result;
+}
+
+void *v4l2_mmap(void *start, size_t length, int prot, int flags, int fd,
+ __off64_t offset)
+{
+ int index;
+ unsigned int buffer_index;
+ void *result;
+
+ if ((index = v4l2_get_index(fd)) == -1 ||
+ /* Check if the mmap data matches our answer to QUERY_BUF, if it doesn't
+ let the kernel handle it (to allow for mmap based non capture use) */
+ start || length != V4L2_FRAME_BUF_SIZE ||
+ ((unsigned int)offset & ~0xFFu) != V4L2_MMAP_OFFSET_MAGIC) {
+ if (index != -1)
+ V4L2_LOG("Passing mmap(%p, %d, ..., %x, through to the driver\n",
+ start, (int)length, (int)offset);
+
+ if (offset & ((1 << MMAP2_PAGE_SHIFT) - 1)) {
+ errno = EINVAL;
+ return MAP_FAILED;
+ }
+
+ return (void *)syscall(SYS_mmap2, start, length, prot, flags, fd,
+ (__off_t)(offset >> MMAP2_PAGE_SHIFT));
+ }
+
+ pthread_mutex_lock(&devices[index].stream_lock);
+
+ buffer_index = offset & 0xff;
+ if (buffer_index >= devices[index].no_frames ||
+ /* Got magic offset and not converting ?? */
+ !v4lconvert_needs_conversion(devices[index].convert,
+ &devices[index].src_fmt, &devices[index].dest_fmt)) {
+ errno = EINVAL;
+ result = MAP_FAILED;
+ goto leave;
+ }
+
+ if (devices[index].convert_mmap_buf == MAP_FAILED) {
+ devices[index].convert_mmap_buf = (void *)syscall(SYS_mmap2, NULL,
+ (size_t)(
+ devices[index].no_frames *
+ V4L2_FRAME_BUF_SIZE),
+ PROT_READ|PROT_WRITE,
+ MAP_ANONYMOUS|MAP_PRIVATE,
+ -1, 0);
+ if (devices[index].convert_mmap_buf == MAP_FAILED) {
+ int saved_err = errno;
+ V4L2_LOG_ERR("allocating conversion buffer\n");
+ errno = saved_err;
+ result = MAP_FAILED;
+ goto leave;
+ }
+ }
+
+ devices[index].frame_map_count[buffer_index]++;
+
+ result = devices[index].convert_mmap_buf +
+ buffer_index * V4L2_FRAME_BUF_SIZE;
+
+ V4L2_LOG("Fake (conversion) mmap buf %u, seen by app at: %p\n",
+ buffer_index, result);
+
+leave:
+ pthread_mutex_unlock(&devices[index].stream_lock);
+
+ return result;
+}
+
+int v4l2_munmap(void *_start, size_t length)
+{
+ int index;
+ unsigned int buffer_index;
+ unsigned char *start = _start;
+
+ /* Is this memory ours? */
+ if (start != MAP_FAILED && length == V4L2_FRAME_BUF_SIZE) {
+ for (index = 0; index < devices_used; index++)
+ if (devices[index].fd != -1 &&
+ devices[index].convert_mmap_buf != MAP_FAILED &&
+ start >= devices[index].convert_mmap_buf &&
+ (start - devices[index].convert_mmap_buf) % length == 0)
+ break;
+
+ if (index != devices_used) {
+ int unmapped = 0;
+
+ pthread_mutex_lock(&devices[index].stream_lock);
+
+ buffer_index = (start - devices[index].convert_mmap_buf) / length;
+
+ /* Redo our checks now that we have the lock, things may have changed */
+ if (devices[index].convert_mmap_buf != MAP_FAILED &&
+ start >= devices[index].convert_mmap_buf &&
+ (start - devices[index].convert_mmap_buf) % length == 0 &&
+ buffer_index < devices[index].no_frames) {
+ if (devices[index].frame_map_count[buffer_index] > 0)
+ devices[index].frame_map_count[buffer_index]--;
+ unmapped = 1;
+ }
+
+ pthread_mutex_unlock(&devices[index].stream_lock);
+
+ if (unmapped) {
+ V4L2_LOG("v4l2 fake buffer munmap %p, %d\n", start, (int)length);
+ return 0;
+ }
+ }
+ }
+
+ V4L2_LOG("v4l2 unknown munmap %p, %d\n", start, (int)length);
+
+ return syscall(SYS_munmap, _start, length);
+}
+
+/* Misc utility functions */
+int v4l2_set_control(int fd, int cid, int value)
+{
+ struct v4l2_queryctrl qctrl = { .id = cid };
+ struct v4l2_control ctrl = { .id = cid };
+ int result;
+
+ if ((result = syscall(SYS_ioctl, fd, VIDIOC_QUERYCTRL, &qctrl)))
+ return result;
+
+ if (!(qctrl.flags & V4L2_CTRL_FLAG_DISABLED) &&
+ !(qctrl.flags & V4L2_CTRL_FLAG_GRABBED)) {
+ if (qctrl.type == V4L2_CTRL_TYPE_BOOLEAN)
+ ctrl.value = value? 1:0;
+ else
+ ctrl.value = (value * (qctrl.maximum - qctrl.minimum) + 32767) / 65535 +
+ qctrl.minimum;
+
+ result = syscall(SYS_ioctl, fd, VIDIOC_S_CTRL, &ctrl);
+ }
+
+ return result;
+}
+
+int v4l2_get_control(int fd, int cid)
+{
+ struct v4l2_queryctrl qctrl = { .id = cid };
+ struct v4l2_control ctrl = { .id = cid };
+
+ if (syscall(SYS_ioctl, fd, VIDIOC_QUERYCTRL, &qctrl))
+ return 0;
+
+ if (qctrl.flags & V4L2_CTRL_FLAG_DISABLED)
+ return 0;
+
+ if (syscall(SYS_ioctl, fd, VIDIOC_G_CTRL, &ctrl))
+ return 0;
+
+ return ((ctrl.value - qctrl.minimum) * 65535 +
+ (qctrl.maximum - qctrl.minimum) / 2) /
+ (qctrl.maximum - qctrl.minimum);
+}
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-04 10:20:51 +0000