diff options
Diffstat (limited to 'v4l2.c')
| -rw-r--r-- | v4l2.c | 733 |
1 files changed, 733 insertions, 0 deletions
@@ -0,0 +1,733 @@ +/* SPDX-License-Identifier: LGPL-2.1-or-later */ +/* + * V4L2 Devices + * + * Copyright (C) 2018 Laurent Pinchart + * + * This file originally comes from the omap3-isp-live project + * (git://git.ideasonboard.org/omap3-isp-live.git) + * + * Copyright (C) 2010-2011 Ideas on board SPRL + * + * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.com> + */ + +#include <errno.h> +#include <fcntl.h> +#include <stdint.h> +#include <stdio.h> +#include <stdlib.h> +#include <string.h> +#include <unistd.h> + +#include <linux/videodev2.h> + +#include <sys/ioctl.h> +#include <sys/mman.h> +#include <sys/select.h> +#include <sys/time.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; + __u32 capabilities; + int ret; + + dev = malloc(sizeof *dev); + if (dev == NULL) + return NULL; + + memset(dev, 0, sizeof *dev); + dev->fd = -1; + dev->name = strdup(devname); + 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 the device_caps field is set use it, otherwise use the older + * capabilities field. + */ + capabilities = cap.device_caps ? : cap.capabilities; + + if (capabilities & V4L2_CAP_VIDEO_CAPTURE) + dev->type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + else if (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); + } + + free(dev->name); + 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("%s: unable to get control (%d).\n", dev->name, 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("%s: unable to set control (%d).\n", dev->name, 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("%s: unable to get multiple controls (%d).\n", dev->name, + 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("%s: unable to set multiple controls (%d).\n", dev->name, + 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("%s: unable to get crop rectangle (%d).\n", dev->name, + 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("%s: unable to set crop rectangle (%d).\n", dev->name, + 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("%s: unable to get format (%d).\n", dev->name, 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("%s: unable to set format (%d).\n", dev->name, errno); + return -errno; + } + + dev->format = fmt.fmt.pix; + *format = fmt.fmt.pix; + + return 0; +} + +/* ----------------------------------------------------------------------------- + * Buffers management + */ + +int v4l2_alloc_buffers(struct v4l2_device *dev, enum v4l2_memory memtype, + unsigned int nbufs) +{ + struct v4l2_requestbuffers rb; + struct v4l2_buffer buf; + unsigned int i; + int ret; + + /* Request the buffers from the driver. */ + memset(&rb, 0, sizeof rb); + rb.count = nbufs; + rb.type = dev->type; + rb.memory = memtype; + + ret = ioctl(dev->fd, VIDIOC_REQBUFS, &rb); + if (ret < 0) { + printf("%s: unable to request buffers (%d).\n", dev->name, + errno); + ret = -errno; + goto done; + } + + if (rb.count > nbufs) { + printf("%s: driver needs more buffers (%u) than available (%u).\n", + dev->name, rb.count, nbufs); + ret = -E2BIG; + goto done; + } + + printf("%s: %u buffers requested.\n", dev->name, rb.count); + + /* Allocate the buffer objects. */ + dev->memtype = memtype; + dev->nbufs = rb.count; + + dev->buffers = malloc(sizeof *dev->buffers * nbufs); + if (dev->buffers == NULL) { + ret = -ENOMEM; + goto done; + } + + memset(dev->buffers, 0, sizeof *dev->buffers * nbufs); + + for (i = 0; i < dev->nbufs; ++i) + dev->buffers[i].index = i; + + /* 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("%s: unable to query buffer %u (%d).\n", + dev->name, i, errno); + ret = -errno; + goto done; + } + + switch (memtype) { + case V4L2_MEMORY_MMAP: + dev->buffers[i].mem = mmap(0, buf.length, PROT_READ | PROT_WRITE, + MAP_SHARED, dev->fd, buf.m.offset); + if (dev->buffers[i].mem == MAP_FAILED) { + printf("%s: unable to map buffer %u (%d)\n", + dev->name, i, errno); + ret = -errno; + goto done; + } + dev->buffers[i].size = buf.length; + printf("%s: buffer %u mapped at address %p.\n", + dev->name, i, dev->buffers[i].mem); + break; + + case V4L2_MEMORY_USERPTR: + if (dev->buffers[i].size < buf.length) { + printf("%s: buffer %u too small (%u bytes required, " + "%u bytes available.\n", dev->name, i, + buf.length, dev->buffers[i].size); + ret = -EINVAL; + goto done; + } + + printf("%s: buffer %u valid.\n", dev->name, 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->nbufs == 0) + return 0; + + if (dev->memtype == V4L2_MEMORY_MMAP) { + for (i = 0; i < dev->nbufs; ++i) { + struct v4l2_video_buffer *buffer = &dev->buffers[i]; + + if (buffer->mem == NULL) + continue; + + ret = munmap(buffer->mem, buffer->size); + if (ret < 0) { + printf("%s: unable to unmap buffer %u (%d)\n", + dev->name, 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("%s: unable to release buffers (%d)\n", dev->name, + errno); + return -errno; + } + + free(dev->buffers); + dev->buffers = NULL; + dev->nbufs = 0; + + 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("%s: unable to dequeue buffer index %u/%u (%d)\n", + dev->name, buf.index, dev->nbufs, errno); + return -errno; + } + + *buffer = dev->buffers[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->buffers[buffer->index].mem; + + buf.length = dev->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("%s: unable to queue buffer index %u/%u (%d)\n", + dev->name, buf.index, dev->nbufs, 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; +} |