/*
* v4l2_device.cpp - v4l2 device
*
* Copyright (c) 2014-2015 Intel Corporation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Author: Wind Yuan <feng.yuan@intel.com>
* Author: John Ye <john.ye@intel.com>
*/
#include "v4l2_device.h"
#include <sys/ioctl.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <sys/mman.h>
#include "v4l2_buffer_proxy.h"
namespace XCam {
#define XCAM_V4L2_DEFAULT_BUFFER_COUNT 6
V4l2Device::V4l2Device (const char *name)
: _name (NULL)
, _fd (-1)
, _sensor_id (0)
, _capture_mode (0)
, _capture_buf_type (V4L2_BUF_TYPE_VIDEO_CAPTURE)
, _memory_type (V4L2_MEMORY_MMAP)
, _fps_n (0)
, _fps_d (0)
, _active (false)
, _buf_count (XCAM_V4L2_DEFAULT_BUFFER_COUNT)
{
if (name)
_name = strndup (name, XCAM_MAX_STR_SIZE);
xcam_mem_clear (_format);
}
V4l2Device::~V4l2Device ()
{
close();
if (_name)
xcam_free (_name);
}
bool
V4l2Device::set_device_name (const char *name)
{
XCAM_ASSERT (name);
if (is_opened()) {
XCAM_LOG_WARNING ("can't set device name since device opened");
return false;
}
if (_name)
xcam_free (_name);
_name = strndup (name, XCAM_MAX_STR_SIZE);
return true;
}
bool
V4l2Device::set_sensor_id (int id)
{
if (is_opened()) {
XCAM_LOG_WARNING ("can't set sensor id since device opened");
return false;
}
_sensor_id = id;
return true;
}
bool
V4l2Device::set_capture_mode (uint32_t capture_mode)
{
if (is_opened()) {
XCAM_LOG_WARNING ("can't set sensor id since device opened");
return false;
}
_capture_mode = capture_mode;
return true;
}
bool
V4l2Device::set_framerate (uint32_t n, uint32_t d)
{
if (_format.fmt.pix.pixelformat) {
XCAM_LOG_WARNING ("device(%s) set framerate failed since formatted was already set.", XCAM_STR(_name));
return false;
}
_fps_n = n;
_fps_d = d;
return true;
}
void
V4l2Device::get_framerate (uint32_t &n, uint32_t &d)
{
n = _fps_n;
d = _fps_d;
}
bool
V4l2Device::set_mem_type (enum v4l2_memory type) {
if (is_activated ()) {
XCAM_LOG_WARNING ("device(%s) set mem type failed", XCAM_STR (_name));
return false;
}
_memory_type = type;
return true;
}
bool
V4l2Device::set_buffer_count (uint32_t buf_count)
{
if (is_activated ()) {
XCAM_LOG_WARNING ("device(%s) set buffer count failed", XCAM_STR (_name));
return false;
}
_buf_count = buf_count;
return true;
}
XCamReturn
V4l2Device::open ()
{
struct v4l2_streamparm param;
if (is_opened()) {
XCAM_LOG_DEBUG ("device(%s) was already opened", XCAM_STR(_name));
return XCAM_RETURN_NO_ERROR;
}
if (!_name) {
XCAM_LOG_DEBUG ("v4l2 device open failed, there's no device name");
return XCAM_RETURN_ERROR_PARAM;
}
_fd = ::open (_name, O_RDWR);
if (_fd == -1) {
XCAM_LOG_DEBUG ("open device(%s) failed", _name);
return XCAM_RETURN_ERROR_IOCTL;
}
// set sensor id
if (io_control (VIDIOC_S_INPUT, &_sensor_id) < 0) {
XCAM_LOG_WARNING ("set sensor id(%d) failed but continue", _sensor_id);
}
// set capture mode
xcam_mem_clear (param);
param.type = _capture_buf_type;
param.parm.capture.capturemode = _capture_mode;
if (io_control (VIDIOC_S_PARM, ¶m) < 0) {
XCAM_LOG_WARNING ("set capture mode(0x%08x) failed but continue", _capture_mode);
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::close ()
{
if (!is_opened())
return XCAM_RETURN_NO_ERROR;
::close (_fd);
_fd = -1;
return XCAM_RETURN_NO_ERROR;
}
int
V4l2Device::io_control (int cmd, void *arg)
{
if (_fd <= 0)
return -1;
return xcam_device_ioctl (_fd, cmd, arg);
}
int
V4l2Device::poll_event (int timeout_msec)
{
struct pollfd poll_fd;
int ret = 0;
XCAM_ASSERT (_fd > 0);
xcam_mem_clear (poll_fd);
poll_fd.fd = _fd;
poll_fd.events = (POLLPRI | POLLIN | POLLERR | POLLNVAL | POLLHUP);
ret = poll (&poll_fd, 1, timeout_msec);
if (ret > 0 && (poll_fd.revents & (POLLERR | POLLNVAL | POLLHUP))) {
XCAM_LOG_DEBUG ("v4l2 subdev(%s) polled error", XCAM_STR(_name));
return -1;
}
return ret;
}
XCamReturn
V4l2Device::set_format (struct v4l2_format &format)
{
XCamReturn ret = XCAM_RETURN_NO_ERROR;
XCAM_FAIL_RETURN (ERROR, !is_activated (), XCAM_RETURN_ERROR_PARAM,
"Cannot set format to v4l2 device while it is active.");
XCAM_FAIL_RETURN (ERROR, is_opened (), XCAM_RETURN_ERROR_FILE,
"Cannot set format to v4l2 device while it is closed.");
struct v4l2_format tmp_format = format;
ret = pre_set_format (format);
if (ret != XCAM_RETURN_NO_ERROR) {
XCAM_LOG_WARNING ("device(%s) pre_set_format failed", XCAM_STR (_name));
return ret;
}
if (io_control (VIDIOC_S_FMT, &format) < 0) {
if (errno == EBUSY) {
// TODO log device name
XCAM_LOG_ERROR("Video device is busy, fail to set format.");
} else {
// TODO log format details and errno
XCAM_LOG_ERROR("Fail to set format: %s", strerror(errno));
}
return XCAM_RETURN_ERROR_IOCTL;
}
if (tmp_format.fmt.pix.width != format.fmt.pix.width || tmp_format.fmt.pix.height != format.fmt.pix.height) {
XCAM_LOG_ERROR (
"device(%s) set v4l2 format failed, supported format: width:%d, height:%d",
XCAM_STR (_name),
format.fmt.pix.width,
format.fmt.pix.height);
return XCAM_RETURN_ERROR_PARAM;
}
while (_fps_n && _fps_d) {
struct v4l2_streamparm param;
xcam_mem_clear (param);
param.type = _capture_buf_type;
if (io_control (VIDIOC_G_PARM, ¶m) < 0) {
XCAM_LOG_WARNING ("device(%s) set framerate failed on VIDIOC_G_PARM but continue", XCAM_STR (_name));
break;
}
if (!(param.parm.capture.capability & V4L2_CAP_TIMEPERFRAME))
break;
param.parm.capture.timeperframe.numerator = _fps_d;
param.parm.capture.timeperframe.denominator = _fps_n;
if (io_control (VIDIOC_S_PARM, ¶m) < 0) {
XCAM_LOG_WARNING ("device(%s) set framerate failed on VIDIOC_S_PARM but continue", XCAM_STR (_name));
break;
}
_fps_n = param.parm.capture.timeperframe.denominator;
_fps_d = param.parm.capture.timeperframe.numerator;
XCAM_LOG_INFO ("device(%s) set framerate(%d/%d)", XCAM_STR (_name), _fps_n, _fps_d);
// exit here, otherwise it is an infinite loop
break;
}
ret = post_set_format (format);
if (ret != XCAM_RETURN_NO_ERROR) {
XCAM_LOG_WARNING ("device(%s) post_set_format failed", XCAM_STR (_name));
return ret;
}
_format = format;
XCAM_LOG_INFO (
"device(%s) set format(w:%d, h:%d, pixelformat:%s, bytesperline:%d,image_size:%d)",
XCAM_STR (_name),
format.fmt.pix.width, format.fmt.pix.height,
xcam_fourcc_to_string (format.fmt.pix.pixelformat),
format.fmt.pix.bytesperline,
format.fmt.pix.sizeimage);
return XCAM_RETURN_NO_ERROR;
}
/*! \brief v4l2 set format
*
* \param[in] width format width
* \param[in] height format height
* \param[in] pixelformat fourcc
* \param[in] field V4L2_FIELD_INTERLACED or V4L2_FIELD_NONE
*/
XCamReturn
V4l2Device::set_format (
uint32_t width, uint32_t height,
uint32_t pixelformat, enum v4l2_field field, uint32_t bytes_perline)
{
struct v4l2_format format;
xcam_mem_clear (format);
format.type = _capture_buf_type;
format.fmt.pix.width = width;
format.fmt.pix.height = height;
format.fmt.pix.pixelformat = pixelformat;
format.fmt.pix.field = field;
if (bytes_perline != 0)
format.fmt.pix.bytesperline = bytes_perline;
return set_format (format);
}
XCamReturn
V4l2Device::pre_set_format (struct v4l2_format &format)
{
XCAM_UNUSED (format);
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::post_set_format (struct v4l2_format &format)
{
XCAM_UNUSED (format);
return XCAM_RETURN_NO_ERROR;
}
std::list<struct v4l2_fmtdesc>
V4l2Device::enum_formats ()
{
std::list<struct v4l2_fmtdesc> formats;
struct v4l2_fmtdesc format;
uint32_t i = 0;
while (1) {
xcam_mem_clear (format);
format.index = i++;
format.type = _capture_buf_type;
if (this->io_control (VIDIOC_ENUM_FMT, &format) < 0) {
if (errno == EINVAL)
break;
else { // error
XCAM_LOG_DEBUG ("enum formats failed");
return formats;
}
}
formats.push_back (format);
}
return formats;
}
XCamReturn
V4l2Device::get_format (struct v4l2_format &format)
{
if (is_activated ()) {
format = _format;
return XCAM_RETURN_NO_ERROR;
}
if (!is_opened ())
return XCAM_RETURN_ERROR_IOCTL;
xcam_mem_clear (format);
format.type = _capture_buf_type;
if (this->io_control (VIDIOC_G_FMT, &format) < 0) {
// FIXME: also log the device name?
XCAM_LOG_ERROR("Fail to get format via ioctl VIDVIO_G_FMT.");
return XCAM_RETURN_ERROR_IOCTL;
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::start ()
{
XCamReturn ret = XCAM_RETURN_NO_ERROR;
// request buffer first
ret = request_buffer ();
XCAM_FAIL_RETURN (
ERROR, ret == XCAM_RETURN_NO_ERROR, ret,
"device(%s) start failed", XCAM_STR (_name));
//alloc buffers
ret = init_buffer_pool ();
XCAM_FAIL_RETURN (
ERROR, ret == XCAM_RETURN_NO_ERROR, ret,
"device(%s) start failed", XCAM_STR (_name));
//queue all buffers
for (uint32_t i = 0; i < _buf_count; ++i) {
SmartPtr<V4l2Buffer> &buf = _buf_pool [i];
XCAM_ASSERT (buf.ptr());
XCAM_ASSERT (buf->get_buf().index == i);
ret = queue_buffer (buf);
if (ret != XCAM_RETURN_NO_ERROR) {
XCAM_LOG_ERROR (
"device(%s) start failed on queue index:%d",
XCAM_STR (_name), i);
stop ();
return ret;
}
}
// stream on
if (io_control (VIDIOC_STREAMON, &_capture_buf_type) < 0) {
XCAM_LOG_ERROR (
"device(%s) start failed on VIDIOC_STREAMON",
XCAM_STR (_name));
stop ();
return XCAM_RETURN_ERROR_IOCTL;
}
_active = true;
XCAM_LOG_INFO ("device(%s) started successfully", XCAM_STR (_name));
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::stop ()
{
// stream off
if (_active) {
if (io_control (VIDIOC_STREAMOFF, &_capture_buf_type) < 0) {
XCAM_LOG_WARNING ("device(%s) steamoff failed", XCAM_STR (_name));
}
_active = false;
}
fini_buffer_pool ();
XCAM_LOG_INFO ("device(%s) stopped", XCAM_STR (_name));
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::request_buffer ()
{
struct v4l2_requestbuffers request_buf;
XCAM_ASSERT (!is_activated());
xcam_mem_clear (request_buf);
request_buf.type = _capture_buf_type;
request_buf.count = _buf_count;
request_buf.memory = _memory_type;
if (io_control (VIDIOC_REQBUFS, &request_buf) < 0) {
XCAM_LOG_INFO ("device(%s) starts failed on VIDIOC_REQBUFS", XCAM_STR (_name));
return XCAM_RETURN_ERROR_IOCTL;
}
if (request_buf.count != _buf_count) {
XCAM_LOG_DEBUG (
"device(%s) request buffer count doesn't match user settings, reset buffer count to %d",
XCAM_STR (_name), request_buf.count);
_buf_count = request_buf.count;
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::allocate_buffer (
SmartPtr<V4l2Buffer> &buf,
const struct v4l2_format &format,
const uint32_t index)
{
struct v4l2_buffer v4l2_buf;
xcam_mem_clear (v4l2_buf);
v4l2_buf.index = index;
v4l2_buf.type = _capture_buf_type;
v4l2_buf.memory = _memory_type;
switch (_memory_type) {
case V4L2_MEMORY_DMABUF:
{
struct v4l2_exportbuffer expbuf;
xcam_mem_clear (expbuf);
expbuf.type = _capture_buf_type;
expbuf.index = index;
expbuf.flags = O_CLOEXEC;
if (io_control (VIDIOC_EXPBUF, &expbuf) < 0) {
XCAM_LOG_WARNING ("device(%s) get dma buf(%d) failed", XCAM_STR (_name), index);
return XCAM_RETURN_ERROR_MEM;
}
v4l2_buf.m.fd = expbuf.fd;
v4l2_buf.length = format.fmt.pix.sizeimage;
}
break;
case V4L2_MEMORY_MMAP:
{
void *pointer;
int map_flags = MAP_SHARED;
#ifdef NEED_MAP_32BIT
map_flags |= MAP_32BIT;
#endif
if (io_control (VIDIOC_QUERYBUF, &v4l2_buf) < 0) {
XCAM_LOG_WARNING("device(%s) query MMAP buf(%d) failed", XCAM_STR(_name), index);
return XCAM_RETURN_ERROR_MEM;
}
pointer = mmap (0, v4l2_buf.length, PROT_READ | PROT_WRITE, map_flags, _fd, v4l2_buf.m.offset);
if (pointer == MAP_FAILED) {
XCAM_LOG_WARNING("device(%s) mmap buf(%d) failed", XCAM_STR(_name), index);
return XCAM_RETURN_ERROR_MEM;
}
v4l2_buf.m.userptr = (uintptr_t) pointer;
}
break;
case V4L2_MEMORY_USERPTR:
default:
XCAM_ASSERT (false);
XCAM_LOG_WARNING (
"device(%s) allocated buffer mem_type(%d) doesn't support",
XCAM_STR (_name), _memory_type);
return XCAM_RETURN_ERROR_MEM;
}
buf = new V4l2Buffer (v4l2_buf, _format);
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::init_buffer_pool ()
{
XCamReturn ret = XCAM_RETURN_NO_ERROR;
uint32_t i = 0;
_buf_pool.clear ();
_buf_pool.reserve (_buf_count);
for (; i < _buf_count; i++) {
SmartPtr<V4l2Buffer> new_buf;
ret = allocate_buffer (new_buf, _format, i);
if (ret != XCAM_RETURN_NO_ERROR) {
break;
}
_buf_pool.push_back (new_buf);
}
if (_buf_pool.empty()) {
XCAM_LOG_ERROR ("No bufer allocated in device(%s)", XCAM_STR (_name));
return XCAM_RETURN_ERROR_MEM;
}
if (i != _buf_count) {
XCAM_LOG_WARNING (
"device(%s) allocate buffer count:%d failback to %d",
XCAM_STR (_name), _buf_count, i);
_buf_count = i;
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::fini_buffer_pool()
{
_buf_pool.clear ();
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::dequeue_buffer(SmartPtr<V4l2Buffer> &buf)
{
struct v4l2_buffer v4l2_buf;
if (!is_activated()) {
XCAM_LOG_DEBUG (
"device(%s) dequeue buffer failed since not activated", XCAM_STR (_name));
return XCAM_RETURN_ERROR_PARAM;
}
xcam_mem_clear (v4l2_buf);
v4l2_buf.type = _capture_buf_type;
v4l2_buf.memory = _memory_type;
if (this->io_control (VIDIOC_DQBUF, &v4l2_buf) < 0) {
XCAM_LOG_ERROR ("device(%s) fail to dequeue buffer.", XCAM_STR (_name));
return XCAM_RETURN_ERROR_IOCTL;
}
XCAM_LOG_DEBUG ("device(%s) dequeue buffer index:%d", XCAM_STR (_name), v4l2_buf.index);
if (v4l2_buf.index > _buf_count) {
XCAM_LOG_ERROR (
"device(%s) dequeue wrong buffer index:%d",
XCAM_STR (_name), v4l2_buf.index);
return XCAM_RETURN_ERROR_ISP;
}
buf = _buf_pool [v4l2_buf.index];
buf->set_timestamp (v4l2_buf.timestamp);
buf->set_timecode (v4l2_buf.timecode);
buf->set_sequence (v4l2_buf.sequence);
//buf.set_length (v4l2_buf.length); // not necessary to set length
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2Device::queue_buffer (SmartPtr<V4l2Buffer> &buf)
{
XCAM_ASSERT (buf.ptr());
buf->reset ();
struct v4l2_buffer v4l2_buf = buf->get_buf ();
XCAM_ASSERT (v4l2_buf.index < _buf_count);
XCAM_LOG_DEBUG ("device(%s) queue buffer index:%d", XCAM_STR (_name), v4l2_buf.index);
if (io_control (VIDIOC_QBUF, &v4l2_buf) < 0) {
XCAM_LOG_ERROR("fail to enqueue buffer index:%d.", v4l2_buf.index);
return XCAM_RETURN_ERROR_IOCTL;
}
return XCAM_RETURN_NO_ERROR;
}
V4l2SubDevice::V4l2SubDevice (const char *name)
: V4l2Device (name)
{
}
XCamReturn
V4l2SubDevice::subscribe_event (int event)
{
struct v4l2_event_subscription sub;
int ret = 0;
XCAM_ASSERT (is_opened());
xcam_mem_clear (sub);
sub.type = event;
ret = this->io_control (VIDIOC_SUBSCRIBE_EVENT, &sub);
if (ret < 0) {
XCAM_LOG_DEBUG ("subdev(%s) subscribe event(%d) failed", XCAM_STR(_name), event);
return XCAM_RETURN_ERROR_IOCTL;
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2SubDevice::unsubscribe_event (int event)
{
struct v4l2_event_subscription sub;
int ret = 0;
XCAM_ASSERT (is_opened());
xcam_mem_clear (sub);
sub.type = event;
ret = this->io_control (VIDIOC_UNSUBSCRIBE_EVENT, &sub);
if (ret < 0) {
XCAM_LOG_DEBUG ("subdev(%s) unsubscribe event(%d) failed", XCAM_STR(_name), event);
return XCAM_RETURN_ERROR_IOCTL;
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn
V4l2SubDevice::dequeue_event (struct v4l2_event &event)
{
int ret = 0;
XCAM_ASSERT (is_opened());
ret = this->io_control (VIDIOC_DQEVENT, &event);
if (ret < 0) {
XCAM_LOG_DEBUG ("subdev(%s) dequeue event failed", XCAM_STR(_name));
return XCAM_RETURN_ERROR_IOCTL;
}
return XCAM_RETURN_NO_ERROR;
}
XCamReturn V4l2SubDevice::start ()
{
if (!is_opened())
return XCAM_RETURN_ERROR_PARAM;
_active = true;
return XCAM_RETURN_NO_ERROR;
}
XCamReturn V4l2SubDevice::stop ()
{
if (_active)
_active = false;
return XCAM_RETURN_NO_ERROR;
}
};