From 0ce8f2390b5280887a2ac179faf2aa01604cb1cc Mon Sep 17 00:00:00 2001 From: Paul Elder Date: Fri, 6 Dec 2019 04:14:52 -0500 Subject: v4l2: v4l2_compat: Add V4L2 compatibility layer Add libcamera V4L2 compatibility layer. This initial implementation supports the minimal set of V4L2 operations, which allows getting, setting, and enumerating formats, and streaming frames from a video device. Some data about the wrapped V4L2 video device are hardcoded. Add a build option named 'v4l2' and adjust the build system to selectively compile the V4L2 compatibility layer. For now we match the V4L2 device node to a libcamera camera based on a devnum that a pipeline handler may optionally map to a libcamera camera. Signed-off-by: Paul Elder Reviewed-by: Jacopo Mondi Reviewed-by: Laurent Pinchart --- meson_options.txt | 5 + src/meson.build | 4 + src/v4l2/meson.build | 31 ++ src/v4l2/v4l2_camera.cpp | 224 ++++++++++++++ src/v4l2/v4l2_camera.h | 85 ++++++ src/v4l2/v4l2_camera_proxy.cpp | 644 +++++++++++++++++++++++++++++++++++++++ src/v4l2/v4l2_camera_proxy.h | 81 +++++ src/v4l2/v4l2_compat.cpp | 85 ++++++ src/v4l2/v4l2_compat_manager.cpp | 250 +++++++++++++++ src/v4l2/v4l2_compat_manager.h | 77 +++++ 10 files changed, 1486 insertions(+) create mode 100644 src/v4l2/meson.build create mode 100644 src/v4l2/v4l2_camera.cpp create mode 100644 src/v4l2/v4l2_camera.h create mode 100644 src/v4l2/v4l2_camera_proxy.cpp create mode 100644 src/v4l2/v4l2_camera_proxy.h create mode 100644 src/v4l2/v4l2_compat.cpp create mode 100644 src/v4l2/v4l2_compat_manager.cpp create mode 100644 src/v4l2/v4l2_compat_manager.h diff --git a/meson_options.txt b/meson_options.txt index 1a328045..79ee4de6 100644 --- a/meson_options.txt +++ b/meson_options.txt @@ -10,3 +10,8 @@ option('documentation', option('test', type : 'boolean', description: 'Compile and include the tests') + +option('v4l2', + type : 'boolean', + value : false, + description : 'Compile the V4L2 compatibility layer') diff --git a/src/meson.build b/src/meson.build index 67ad20aa..5adcd61f 100644 --- a/src/meson.build +++ b/src/meson.build @@ -6,3 +6,7 @@ subdir('libcamera') subdir('ipa') subdir('cam') subdir('qcam') + +if get_option('v4l2') + subdir('v4l2') +endif diff --git a/src/v4l2/meson.build b/src/v4l2/meson.build new file mode 100644 index 00000000..14ee3594 --- /dev/null +++ b/src/v4l2/meson.build @@ -0,0 +1,31 @@ +v4l2_compat_sources = files([ + 'v4l2_camera.cpp', + 'v4l2_camera_proxy.cpp', + 'v4l2_compat.cpp', + 'v4l2_compat_manager.cpp', +]) + +v4l2_compat_includes = [ + libcamera_includes, + libcamera_internal_includes, +] + +v4l2_compat_cpp_args = [ + # Meson enables large file support unconditionally, which redirect file + # operations to 64-bit versions. This results in some symbols being + # renamed, for instance open() being renamed to open64(). As the V4L2 + # adaptation wrapper needs to provide both 32-bit and 64-bit versions of + # file operations, disable transparent large file support. + '-U_FILE_OFFSET_BITS', + '-D_FILE_OFFSET_BITS=32', + '-fvisibility=hidden', +] + +v4l2_compat = shared_library('v4l2-compat', + v4l2_compat_sources, + name_prefix : '', + install : true, + link_with : libcamera, + include_directories : v4l2_compat_includes, + dependencies : libcamera_deps, + cpp_args : v4l2_compat_cpp_args) diff --git a/src/v4l2/v4l2_camera.cpp b/src/v4l2/v4l2_camera.cpp new file mode 100644 index 00000000..3590730f --- /dev/null +++ b/src/v4l2/v4l2_camera.cpp @@ -0,0 +1,224 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2019, Google Inc. + * + * v4l2_camera.cpp - V4L2 compatibility camera + */ + +#include "v4l2_camera.h" + +#include + +#include "log.h" +#include "utils.h" + +using namespace libcamera; + +LOG_DECLARE_CATEGORY(V4L2Compat); + +FrameMetadata::FrameMetadata(Buffer *buffer) + : index_(buffer->index()), bytesused_(buffer->bytesused()), + timestamp_(buffer->timestamp()), sequence_(buffer->sequence()), + status_(buffer->status()) +{ +} + +V4L2Camera::V4L2Camera(std::shared_ptr camera) + : camera_(camera), isRunning_(false) +{ + camera_->requestCompleted.connect(this, &V4L2Camera::requestComplete); +} + +V4L2Camera::~V4L2Camera() +{ + camera_->release(); +} + +void V4L2Camera::open(int *ret) +{ + /* \todo Support multiple open. */ + if (camera_->acquire() < 0) { + LOG(V4L2Compat, Error) << "Failed to acquire camera"; + *ret = -EINVAL; + return; + } + + config_ = camera_->generateConfiguration({ StreamRole::Viewfinder }); + if (!config_) { + camera_->release(); + *ret = -EINVAL; + return; + } + + *ret = 0; +} + +void V4L2Camera::close() +{ + camera_->release(); +} + +void V4L2Camera::getStreamConfig(StreamConfiguration *streamConfig) +{ + *streamConfig = config_->at(0); +} + +std::vector V4L2Camera::completedBuffers() +{ + std::vector v; + + bufferLock_.lock(); + for (std::unique_ptr &metadata : completedBuffers_) + v.push_back(*metadata.get()); + completedBuffers_.clear(); + bufferLock_.unlock(); + + return v; +} + +void V4L2Camera::requestComplete(Request *request) +{ + if (request->status() == Request::RequestCancelled) + return; + + /* We only have one stream at the moment. */ + bufferLock_.lock(); + Buffer *buffer = request->buffers().begin()->second; + std::unique_ptr metadata = + utils::make_unique(buffer); + completedBuffers_.push_back(std::move(metadata)); + bufferLock_.unlock(); + + bufferSema_.release(); +} + +void V4L2Camera::configure(int *ret, StreamConfiguration *streamConfigOut, + const Size &size, PixelFormat pixelformat, + unsigned int bufferCount) +{ + StreamConfiguration &streamConfig = config_->at(0); + streamConfig.size.width = size.width; + streamConfig.size.height = size.height; + streamConfig.pixelFormat = pixelformat; + streamConfig.bufferCount = bufferCount; + /* \todo memoryType (interval vs external) */ + + CameraConfiguration::Status validation = config_->validate(); + if (validation == CameraConfiguration::Invalid) { + LOG(V4L2Compat, Debug) << "Configuration invalid"; + *ret = -EINVAL; + return; + } + if (validation == CameraConfiguration::Adjusted) + LOG(V4L2Compat, Debug) << "Configuration adjusted"; + + LOG(V4L2Compat, Debug) << "Validated configuration is: " + << streamConfig.toString(); + + *ret = camera_->configure(config_.get()); + if (*ret < 0) + return; + + *streamConfigOut = config_->at(0); +} + +void V4L2Camera::mmap(void **ret, unsigned int index) +{ + Stream *stream = *camera_->streams().begin(); + *ret = stream->buffers()[index].planes()[0].mem(); +} + +void V4L2Camera::allocBuffers(int *ret, unsigned int count) +{ + *ret = camera_->allocateBuffers(); + if (*ret == -EACCES) + *ret = -EBUSY; +} + +void V4L2Camera::freeBuffers() +{ + camera_->freeBuffers(); +} + +void V4L2Camera::streamOn(int *ret) +{ + *ret = 0; + + if (isRunning_) + return; + + *ret = camera_->start(); + if (*ret < 0) { + if (*ret == -EACCES) + *ret = -EBUSY; + return; + } + isRunning_ = true; + + for (std::unique_ptr &req : pendingRequests_) { + /* \todo What should we do if this returns -EINVAL? */ + *ret = camera_->queueRequest(req.release()); + if (*ret < 0) { + if (*ret == -EACCES) + *ret = -EBUSY; + return; + } + } + + pendingRequests_.clear(); +} + +void V4L2Camera::streamOff(int *ret) +{ + *ret = 0; + + /* \todo Restore buffers to reqbufs state? */ + if (!isRunning_) + return; + + *ret = camera_->stop(); + if (*ret < 0) { + if (*ret == -EACCES) + *ret = -EBUSY; + return; + } + isRunning_ = false; +} + +void V4L2Camera::qbuf(int *ret, unsigned int index) +{ + Stream *stream = config_->at(0).stream(); + std::unique_ptr buffer = stream->createBuffer(index); + if (!buffer) { + LOG(V4L2Compat, Error) << "Can't create buffer"; + *ret = -ENOMEM; + return; + } + + std::unique_ptr request = + std::unique_ptr(camera_->createRequest()); + if (!request) { + LOG(V4L2Compat, Error) << "Can't create request"; + *ret = -ENOMEM; + return; + } + + *ret = request->addBuffer(std::move(buffer)); + if (*ret < 0) { + LOG(V4L2Compat, Error) << "Can't set buffer for request"; + *ret = -ENOMEM; + return; + } + + if (!isRunning_) { + pendingRequests_.push_back(std::move(request)); + return; + } + + *ret = camera_->queueRequest(request.release()); + if (*ret < 0) { + LOG(V4L2Compat, Error) << "Can't queue request"; + if (*ret == -EACCES) + *ret = -EBUSY; + } +} diff --git a/src/v4l2/v4l2_camera.h b/src/v4l2/v4l2_camera.h new file mode 100644 index 00000000..d24dbca6 --- /dev/null +++ b/src/v4l2/v4l2_camera.h @@ -0,0 +1,85 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2019, Google Inc. + * + * v4l2_camera.h - V4L2 compatibility camera + */ + +#ifndef __V4L2_CAMERA_H__ +#define __V4L2_CAMERA_H__ + +#include +#include +#include + +#include +#include + +#include "semaphore.h" + +using namespace libcamera; + +class FrameMetadata +{ +public: + FrameMetadata(Buffer *buffer); + + int index() const { return index_; } + + unsigned int bytesused() const { return bytesused_; } + uint64_t timestamp() const { return timestamp_; } + unsigned int sequence() const { return sequence_; } + + Buffer::Status status() const { return status_; } + +private: + int index_; + + unsigned int bytesused_; + uint64_t timestamp_; + unsigned int sequence_; + + Buffer::Status status_; +}; + +class V4L2Camera : public Object +{ +public: + V4L2Camera(std::shared_ptr camera); + ~V4L2Camera(); + + void open(int *ret); + void close(); + void getStreamConfig(StreamConfiguration *streamConfig); + std::vector completedBuffers(); + + void mmap(void **ret, unsigned int index); + + void configure(int *ret, StreamConfiguration *streamConfigOut, + const Size &size, PixelFormat pixelformat, + unsigned int bufferCount); + + void allocBuffers(int *ret, unsigned int count); + void freeBuffers(); + void streamOn(int *ret); + void streamOff(int *ret); + + void qbuf(int *ret, unsigned int index); + + Semaphore bufferSema_; + +private: + void requestComplete(Request *request); + + std::shared_ptr camera_; + std::unique_ptr config_; + + bool isRunning_; + + std::mutex bufferLock_; + + std::deque> pendingRequests_; + std::deque> completedBuffers_; +}; + +#endif /* __V4L2_CAMERA_H__ */ diff --git a/src/v4l2/v4l2_camera_proxy.cpp b/src/v4l2/v4l2_camera_proxy.cpp new file mode 100644 index 00000000..559caba6 --- /dev/null +++ b/src/v4l2/v4l2_camera_proxy.cpp @@ -0,0 +1,644 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2019, Google Inc. + * + * v4l2_camera_proxy.cpp - Proxy to V4L2 compatibility camera + */ + +#include "v4l2_camera_proxy.h" + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "log.h" +#include "utils.h" +#include "v4l2_camera.h" +#include "v4l2_compat_manager.h" + +#define KERNEL_VERSION(a, b, c) (((a) << 16) + ((b) << 8) + (c)) + +using namespace libcamera; + +LOG_DECLARE_CATEGORY(V4L2Compat); + +V4L2CameraProxy::V4L2CameraProxy(unsigned int index, + std::shared_ptr camera) + : refcount_(0), index_(index), bufferCount_(0), currentBuf_(0), + vcam_(utils::make_unique(camera)) +{ + querycap(camera); +} + +int V4L2CameraProxy::open(bool nonBlocking) +{ + LOG(V4L2Compat, Debug) << "Servicing open"; + + int ret; + vcam_->invokeMethod(&V4L2Camera::open, ConnectionTypeBlocking, + &ret); + if (ret < 0) { + errno = -ret; + return -1; + } + + nonBlocking_ = nonBlocking; + + vcam_->invokeMethod(&V4L2Camera::getStreamConfig, + ConnectionTypeBlocking, &streamConfig_); + setFmtFromConfig(streamConfig_); + sizeimage_ = calculateSizeImage(streamConfig_); + + refcount_++; + + return 0; +} + +void V4L2CameraProxy::dup() +{ + refcount_++; +} + +void V4L2CameraProxy::close() +{ + LOG(V4L2Compat, Debug) << "Servicing close"; + + if (--refcount_ > 0) + return; + + vcam_->invokeMethod(&V4L2Camera::close, ConnectionTypeBlocking); +} + +void *V4L2CameraProxy::mmap(size_t length, int prot, int flags, off_t offset) +{ + LOG(V4L2Compat, Debug) << "Servicing mmap"; + + /* \todo Validate prot and flags properly. */ + if (prot != (PROT_READ | PROT_WRITE)) { + errno = EINVAL; + return MAP_FAILED; + } + + unsigned int index = offset / sizeimage_; + if (index * sizeimage_ != offset || length != sizeimage_) { + errno = EINVAL; + return MAP_FAILED; + } + + void *val; + vcam_->invokeMethod(&V4L2Camera::mmap, ConnectionTypeBlocking, + &val, index); + + buffers_[index].flags |= V4L2_BUF_FLAG_MAPPED; + mmaps_[val] = index; + + return val; +} + +int V4L2CameraProxy::munmap(void *addr, size_t length) +{ + LOG(V4L2Compat, Debug) << "Servicing munmap"; + + auto iter = mmaps_.find(addr); + if (iter == mmaps_.end() || length != sizeimage_) { + errno = EINVAL; + return -1; + } + + buffers_[iter->second].flags &= ~V4L2_BUF_FLAG_MAPPED; + mmaps_.erase(iter); + + return 0; +} + +bool V4L2CameraProxy::validateBufferType(uint32_t type) +{ + return type == V4L2_BUF_TYPE_VIDEO_CAPTURE; +} + +bool V4L2CameraProxy::validateMemoryType(uint32_t memory) +{ + return memory == V4L2_MEMORY_MMAP; +} + +void V4L2CameraProxy::setFmtFromConfig(StreamConfiguration &streamConfig) +{ + curV4L2Format_.fmt.pix.width = streamConfig.size.width; + curV4L2Format_.fmt.pix.height = streamConfig.size.height; + curV4L2Format_.fmt.pix.pixelformat = drmToV4L2(streamConfig.pixelFormat); + curV4L2Format_.fmt.pix.field = V4L2_FIELD_NONE; + curV4L2Format_.fmt.pix.bytesperline = + bplMultiplier(curV4L2Format_.fmt.pix.pixelformat) * + curV4L2Format_.fmt.pix.width; + curV4L2Format_.fmt.pix.sizeimage = + imageSize(curV4L2Format_.fmt.pix.pixelformat, + curV4L2Format_.fmt.pix.width, + curV4L2Format_.fmt.pix.height); + curV4L2Format_.fmt.pix.colorspace = V4L2_COLORSPACE_SRGB; +} + +unsigned int V4L2CameraProxy::calculateSizeImage(StreamConfiguration &streamConfig) +{ + /* + * \todo Merge this method with setFmtFromConfig (need imageSize to + * support all libcamera formats first, or filter out MJPEG for now). + */ + return imageSize(drmToV4L2(streamConfig.pixelFormat), + streamConfig.size.width, + streamConfig.size.height); +} + +void V4L2CameraProxy::querycap(std::shared_ptr camera) +{ + std::string driver = "libcamera"; + std::string bus_info = driver + ":" + std::to_string(index_); + + utils::strlcpy(reinterpret_cast(capabilities_.driver), driver.c_str(), + sizeof(capabilities_.driver)); + utils::strlcpy(reinterpret_cast(capabilities_.card), camera->name().c_str(), + sizeof(capabilities_.card)); + utils::strlcpy(reinterpret_cast(capabilities_.bus_info), bus_info.c_str(), + sizeof(capabilities_.bus_info)); + /* \todo Put this in a header/config somewhere. */ + capabilities_.version = KERNEL_VERSION(5, 2, 0); + capabilities_.device_caps = V4L2_CAP_VIDEO_CAPTURE; + capabilities_.capabilities = capabilities_.device_caps + | V4L2_CAP_DEVICE_CAPS; + memset(capabilities_.reserved, 0, sizeof(capabilities_.reserved)); +} + +void V4L2CameraProxy::updateBuffers() +{ + std::vector completedBuffers = vcam_->completedBuffers(); + for (FrameMetadata &fmd : completedBuffers) { + struct v4l2_buffer &buf = buffers_[fmd.index()]; + + switch (fmd.status()) { + case Buffer::Status::BufferSuccess: + buf.bytesused = fmd.bytesused(); + buf.field = V4L2_FIELD_NONE; + buf.timestamp.tv_sec = fmd.timestamp() / 1000000000; + buf.timestamp.tv_usec = fmd.timestamp() % 1000000; + buf.sequence = fmd.sequence(); + + buf.flags |= V4L2_BUF_FLAG_DONE; + break; + case Buffer::Status::BufferError: + buf.flags |= V4L2_BUF_FLAG_ERROR; + break; + default: + break; + } + } +} + +int V4L2CameraProxy::vidioc_querycap(struct v4l2_capability *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_querycap"; + + *arg = capabilities_; + + return 0; +} + +int V4L2CameraProxy::vidioc_enum_fmt(struct v4l2_fmtdesc *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_enum_fmt"; + + if (!validateBufferType(arg->type) || + arg->index > streamConfig_.formats().pixelformats().size()) + return -EINVAL; + + /* \todo Add map from format to description. */ + utils::strlcpy(reinterpret_cast(arg->description), "Video Format Description", + sizeof(arg->description)); + arg->pixelformat = drmToV4L2(streamConfig_.formats().pixelformats()[arg->index]); + + return 0; +} + +int V4L2CameraProxy::vidioc_g_fmt(struct v4l2_format *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_g_fmt"; + + if (!validateBufferType(arg->type)) + return -EINVAL; + + memset(&arg->fmt, 0, sizeof(arg->fmt)); + arg->fmt.pix = curV4L2Format_.fmt.pix; + + return 0; +} + +int V4L2CameraProxy::vidioc_s_fmt(struct v4l2_format *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_s_fmt"; + + int ret = vidioc_try_fmt(arg); + if (ret < 0) + return ret; + + Size size(arg->fmt.pix.width, arg->fmt.pix.height); + vcam_->invokeMethod(&V4L2Camera::configure, ConnectionTypeBlocking, + &ret, &streamConfig_, size, + v4l2ToDrm(arg->fmt.pix.pixelformat), bufferCount_); + if (ret < 0) + return -EINVAL; + + unsigned int sizeimage = calculateSizeImage(streamConfig_); + if (sizeimage == 0) + return -EINVAL; + + sizeimage_ = sizeimage; + + setFmtFromConfig(streamConfig_); + + return 0; +} + +int V4L2CameraProxy::vidioc_try_fmt(struct v4l2_format *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_try_fmt"; + if (!validateBufferType(arg->type)) + return -EINVAL; + + PixelFormat format = v4l2ToDrm(arg->fmt.pix.pixelformat); + const std::vector &formats = + streamConfig_.formats().pixelformats(); + if (std::find(formats.begin(), formats.end(), format) == formats.end()) + format = streamConfig_.formats().pixelformats()[0]; + + Size size(arg->fmt.pix.width, arg->fmt.pix.height); + const std::vector &sizes = streamConfig_.formats().sizes(format); + if (std::find(sizes.begin(), sizes.end(), size) == sizes.end()) + size = streamConfig_.formats().sizes(format)[0]; + + arg->fmt.pix.width = size.width; + arg->fmt.pix.height = size.height; + arg->fmt.pix.pixelformat = v4l2ToDrm(format); + arg->fmt.pix.field = V4L2_FIELD_NONE; + arg->fmt.pix.bytesperline = bplMultiplier(drmToV4L2(format)) * + arg->fmt.pix.width; + arg->fmt.pix.sizeimage = imageSize(drmToV4L2(format), + arg->fmt.pix.width, + arg->fmt.pix.height); + arg->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB; + + return 0; +} + +int V4L2CameraProxy::freeBuffers() +{ + LOG(V4L2Compat, Debug) << "Freeing libcamera bufs"; + + int ret; + vcam_->invokeMethod(&V4L2Camera::streamOff, + ConnectionTypeBlocking, &ret); + if (ret < 0) { + LOG(V4L2Compat, Error) << "Failed to stop stream"; + return ret; + } + vcam_->invokeMethod(&V4L2Camera::freeBuffers, ConnectionTypeBlocking); + bufferCount_ = 0; + + return 0; +} + +int V4L2CameraProxy::vidioc_reqbufs(struct v4l2_requestbuffers *arg) +{ + int ret; + + LOG(V4L2Compat, Debug) << "Servicing vidioc_reqbufs"; + if (!validateBufferType(arg->type) || + !validateMemoryType(arg->memory)) + return -EINVAL; + + LOG(V4L2Compat, Debug) << arg->count << " buffers requested "; + + arg->capabilities = V4L2_BUF_CAP_SUPPORTS_MMAP; + + if (arg->count == 0) + return freeBuffers(); + + Size size(curV4L2Format_.fmt.pix.width, curV4L2Format_.fmt.pix.height); + vcam_->invokeMethod(&V4L2Camera::configure, ConnectionTypeBlocking, + &ret, &streamConfig_, size, + v4l2ToDrm(curV4L2Format_.fmt.pix.pixelformat), + arg->count); + if (ret < 0) + return -EINVAL; + + sizeimage_ = calculateSizeImage(streamConfig_); + if (sizeimage_ == 0) + return -EINVAL; + + setFmtFromConfig(streamConfig_); + + arg->count = streamConfig_.bufferCount; + bufferCount_ = arg->count; + + vcam_->invokeMethod(&V4L2Camera::allocBuffers, + ConnectionTypeBlocking, &ret, arg->count); + if (ret < 0) { + arg->count = 0; + return ret; + } + + buffers_.resize(arg->count); + for (unsigned int i = 0; i < arg->count; i++) { + struct v4l2_buffer buf = {}; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.length = curV4L2Format_.fmt.pix.sizeimage; + buf.memory = V4L2_MEMORY_MMAP; + buf.m.offset = i * curV4L2Format_.fmt.pix.sizeimage; + buf.index = i; + + buffers_[i] = buf; + } + + LOG(V4L2Compat, Debug) << "Allocated " << arg->count << " buffers"; + + return 0; +} + +int V4L2CameraProxy::vidioc_querybuf(struct v4l2_buffer *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_querybuf"; + + if (!validateBufferType(arg->type) || + arg->index >= bufferCount_) + return -EINVAL; + + updateBuffers(); + + *arg = buffers_[arg->index]; + + return 0; +} + +int V4L2CameraProxy::vidioc_qbuf(struct v4l2_buffer *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_qbuf, index = " + << arg->index; + + if (!validateBufferType(arg->type) || + !validateMemoryType(arg->memory) || + arg->index >= bufferCount_) + return -EINVAL; + + int ret; + vcam_->invokeMethod(&V4L2Camera::qbuf, ConnectionTypeBlocking, + &ret, arg->index); + if (ret < 0) + return ret; + + arg->flags |= V4L2_BUF_FLAG_QUEUED; + arg->flags &= ~V4L2_BUF_FLAG_DONE; + + return ret; +} + +int V4L2CameraProxy::vidioc_dqbuf(struct v4l2_buffer *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_dqbuf"; + + if (!validateBufferType(arg->type) || + !validateMemoryType(arg->memory)) + return -EINVAL; + + if (nonBlocking_ && !vcam_->bufferSema_.tryAcquire()) + return -EAGAIN; + else + vcam_->bufferSema_.acquire(); + + updateBuffers(); + + struct v4l2_buffer &buf = buffers_[currentBuf_]; + + buf.flags &= ~V4L2_BUF_FLAG_QUEUED; + buf.length = sizeimage_; + *arg = buf; + + currentBuf_ = (currentBuf_ + 1) % bufferCount_; + + return 0; +} + +int V4L2CameraProxy::vidioc_streamon(int *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_streamon"; + + if (!validateBufferType(*arg)) + return -EINVAL; + + int ret; + vcam_->invokeMethod(&V4L2Camera::streamOn, + ConnectionTypeBlocking, &ret); + + return ret; +} + +int V4L2CameraProxy::vidioc_streamoff(int *arg) +{ + LOG(V4L2Compat, Debug) << "Servicing vidioc_streamoff"; + + if (!validateBufferType(*arg)) + return -EINVAL; + + int ret; + vcam_->invokeMethod(&V4L2Camera::streamOff, + ConnectionTypeBlocking, &ret); + + for (struct v4l2_buffer &buf : buffers_) + buf.flags &= ~(V4L2_BUF_FLAG_QUEUED | V4L2_BUF_FLAG_DONE); + + return ret; +} + +int V4L2CameraProxy::ioctl(unsigned long request, void *arg) +{ + int ret; + switch (request) { + case VIDIOC_QUERYCAP: + ret = vidioc_querycap(static_cast(arg)); + break; + case VIDIOC_ENUM_FMT: + ret = vidioc_enum_fmt(static_cast(arg)); + break; + case VIDIOC_G_FMT: + ret = vidioc_g_fmt(static_cast(arg)); + break; + case VIDIOC_S_FMT: + ret = vidioc_s_fmt(static_cast(arg)); + break; + case VIDIOC_TRY_FMT: + ret = vidioc_try_fmt(static_cast(arg)); + break; + case VIDIOC_REQBUFS: + ret = vidioc_reqbufs(static_cast(arg)); + break; + case VIDIOC_QUERYBUF: + ret = vidioc_querybuf(static_cast(arg)); + break; + case VIDIOC_QBUF: + ret = vidioc_qbuf(static_cast(arg)); + break; + case VIDIOC_DQBUF: + ret = vidioc_dqbuf(static_cast(arg)); + break; + case VIDIOC_STREAMON: + ret = vidioc_streamon(static_cast(arg)); + break; + case VIDIOC_STREAMOFF: + ret = vidioc_streamoff(static_cast(arg)); + break; + default: + ret = -ENOTTY; + break; + } + + if (ret < 0) { + errno = -ret; + return -1; + } + + return ret; +} + +/* \todo make libcamera export these */ +int V4L2CameraProxy::bplMultiplier(unsigned int format) +{ + switch (format) { + case V4L2_PIX_FMT_NV12: + case V4L2_PIX_FMT_NV21: + case V4L2_PIX_FMT_NV16: + case V4L2_PIX_FMT_NV61: + case V4L2_PIX_FMT_NV24: + case V4L2_PIX_FMT_NV42: + return 1; + case V4L2_PIX_FMT_BGR24: + case V4L2_PIX_FMT_RGB24: + return 3; + case V4L2_PIX_FMT_ARGB32: + return 4; + case V4L2_PIX_FMT_VYUY: + case V4L2_PIX_FMT_YVYU: + case V4L2_PIX_FMT_UYVY: + case V4L2_PIX_FMT_YUYV: + return 2; + default: + return 0; + }; +} + +int V4L2CameraProxy::imageSize(unsigned int format, + unsigned int width, unsigned int height) +{ + switch (format) { + case V4L2_PIX_FMT_NV12: + case V4L2_PIX_FMT_NV21: + return width * height * 3 / 2; + case V4L2_PIX_FMT_NV16: + case V4L2_PIX_FMT_NV61: + return width * height * 2; + case V4L2_PIX_FMT_NV24: + case V4L2_PIX_FMT_NV42: + return width * height * 3; + case V4L2_PIX_FMT_BGR24: + case V4L2_PIX_FMT_RGB24: + return width * height * 3; + case V4L2_PIX_FMT_ARGB32: + return width * height * 4; + case V4L2_PIX_FMT_VYUY: + case V4L2_PIX_FMT_YVYU: + case V4L2_PIX_FMT_UYVY: + case V4L2_PIX_FMT_YUYV: + return width * height * 2; + default: + return 0; + }; +} + +unsigned int V4L2CameraProxy::v4l2ToDrm(unsigned int pixelformat) +{ + switch (pixelformat) { + /* RGB formats. */ + case V4L2_PIX_FMT_RGB24: + return DRM_FORMAT_BGR888; + case V4L2_PIX_FMT_BGR24: + return DRM_FORMAT_RGB888; + case V4L2_PIX_FMT_ARGB32: + return DRM_FORMAT_BGRA8888; + + /* YUV packed formats. */ + case V4L2_PIX_FMT_YUYV: + return DRM_FORMAT_YUYV; + case V4L2_PIX_FMT_YVYU: + return DRM_FORMAT_YVYU; + case V4L2_PIX_FMT_UYVY: + return DRM_FORMAT_UYVY; + case V4L2_PIX_FMT_VYUY: + return DRM_FORMAT_VYUY; + + /* YUY planar formats. */ + case V4L2_PIX_FMT_NV16: + return DRM_FORMAT_NV16; + case V4L2_PIX_FMT_NV61: + return DRM_FORMAT_NV61; + case V4L2_PIX_FMT_NV12: + return DRM_FORMAT_NV12; + case V4L2_PIX_FMT_NV21: + return DRM_FORMAT_NV21; + case V4L2_PIX_FMT_NV24: + return DRM_FORMAT_NV24; + case V4L2_PIX_FMT_NV42: + return DRM_FORMAT_NV42; + default: + return pixelformat; + }; +} + +unsigned int V4L2CameraProxy::drmToV4L2(unsigned int pixelformat) +{ + switch (pixelformat) { + /* RGB formats. */ + case DRM_FORMAT_BGR888: + return V4L2_PIX_FMT_RGB24; + case DRM_FORMAT_RGB888: + return V4L2_PIX_FMT_BGR24; + case DRM_FORMAT_BGRA8888: + return V4L2_PIX_FMT_ARGB32; + + /* YUV packed formats. */ + case DRM_FORMAT_YUYV: + return V4L2_PIX_FMT_YUYV; + case DRM_FORMAT_YVYU: + return V4L2_PIX_FMT_YVYU; + case DRM_FORMAT_UYVY: + return V4L2_PIX_FMT_UYVY; + case DRM_FORMAT_VYUY: + return V4L2_PIX_FMT_VYUY; + + /* YUY planar formats. */ + case DRM_FORMAT_NV16: + return V4L2_PIX_FMT_NV16; + case DRM_FORMAT_NV61: + return V4L2_PIX_FMT_NV61; + case DRM_FORMAT_NV12: + return V4L2_PIX_FMT_NV12; + case DRM_FORMAT_NV21: + return V4L2_PIX_FMT_NV21; + case DRM_FORMAT_NV24: + return V4L2_PIX_FMT_NV24; + case DRM_FORMAT_NV42: + return V4L2_PIX_FMT_NV42; + default: + return pixelformat; + } +} diff --git a/src/v4l2/v4l2_camera_proxy.h b/src/v4l2/v4l2_camera_proxy.h new file mode 100644 index 00000000..bef0f0af --- /dev/null +++ b/src/v4l2/v4l2_camera_proxy.h @@ -0,0 +1,81 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2019, Google Inc. + * + * v4l2_camera_proxy.h - Proxy to V4L2 compatibility camera + */ + +#ifndef __V4L2_CAMERA_PROXY_H__ +#define __V4L2_CAMERA_PROXY_H__ + +#include +#include +#include +#include +#include + +#include + +#include "v4l2_camera.h" + +using namespace libcamera; + +class V4L2CameraProxy +{ +public: + V4L2CameraProxy(unsigned int index, std::shared_ptr camera); + + int open(bool nonBlocking); + void dup(); + void close(); + void *mmap(size_t length, int prot, int flags, off_t offset); + int munmap(void *addr, size_t length); + + int ioctl(unsigned long request, void *arg); + +private: + bool validateBufferType(uint32_t type); + bool validateMemoryType(uint32_t memory); + void setFmtFromConfig(StreamConfiguration &streamConfig); + unsigned int calculateSizeImage(StreamConfiguration &streamConfig); + void querycap(std::shared_ptr camera); + void updateBuffers(); + int freeBuffers(); + + int vidioc_querycap(struct v4l2_capability *arg); + int vidioc_enum_fmt(struct v4l2_fmtdesc *arg); + int vidioc_g_fmt(struct v4l2_format *arg); + int vidioc_s_fmt(struct v4l2_format *arg); + int vidioc_try_fmt(struct v4l2_format *arg); + int vidioc_reqbufs(struct v4l2_requestbuffers *arg); + int vidioc_querybuf(struct v4l2_buffer *arg); + int vidioc_qbuf(struct v4l2_buffer *arg); + int vidioc_dqbuf(struct v4l2_buffer *arg); + int vidioc_streamon(int *arg); + int vidioc_streamoff(int *arg); + + static int bplMultiplier(unsigned int format); + static int imageSize(unsigned int format, unsigned int width, + unsigned int height); + + static unsigned int v4l2ToDrm(unsigned int pixelformat); + static unsigned int drmToV4L2(unsigned int pixelformat); + + unsigned int refcount_; + unsigned int index_; + bool nonBlocking_; + + struct v4l2_format curV4L2Format_; + StreamConfiguration streamConfig_; + struct v4l2_capability capabilities_; + unsigned int bufferCount_; + unsigned int currentBuf_; + unsigned int sizeimage_; + + std::vector buffers_; + std::map mmaps_; + + std::unique_ptr vcam_; +}; + +#endif /* __V4L2_CAMERA_PROXY_H__ */ diff --git a/src/v4l2/v4l2_compat.cpp b/src/v4l2/v4l2_compat.cpp new file mode 100644 index 00000000..171ebed6 --- /dev/null +++ b/src/v4l2/v4l2_compat.cpp @@ -0,0 +1,85 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2019, Google Inc. + * + * v4l2_compat.cpp - V4L2 compatibility layer + */ + +#include "v4l2_compat_manager.h" + +#include +#include +#include +#include +#include +#include + +#define LIBCAMERA_PUBLIC __attribute__((visibility("default"))) + +using namespace libcamera; + +#define extract_va_arg(type, arg, last) \ +{ \ + va_list ap; \ + va_start(ap, last); \ + arg = va_arg(ap, type); \ + va_end(ap); \ +} + +extern "C" { + +LIBCAMERA_PUBLIC int open(const char *path, int oflag, ...) +{ + mode_t mode = 0; + if (oflag & O_CREAT || oflag & O_TMPFILE) + extract_va_arg(mode_t, mode, oflag); + + return V4L2CompatManager::instance()->openat(AT_FDCWD, path, + oflag, mode); +} + +/* _FORTIFY_SOURCE redirects open to __open_2 */ +LIBCAMERA_PUBLIC extern __typeof(open) __open_2 __attribute__ ((alias("open"))); + +LIBCAMERA_PUBLIC int openat(int dirfd, const char *path, int oflag, ...) +{ + mode_t mode = 0; + if (oflag & O_CREAT || oflag & O_TMPFILE) + extract_va_arg(mode_t, mode, oflag); + + return V4L2CompatManager::instance()->openat(dirfd, path, oflag, mode); +} + +LIBCAMERA_PUBLIC extern __typeof(openat) __openat_2 __attribute__ ((alias("openat"))); + +LIBCAMERA_PUBLIC int dup(int oldfd) +{ + return V4L2CompatManager::instance()->dup(oldfd); +} + +LIBCAMERA_PUBLIC int close(int fd) +{ + return V4L2CompatManager::instance()->close(fd); +} + +LIBCAMERA_PUBLIC void *mmap(void *addr, size_t length, int prot, int flags, + int fd, off_t offset) +{ + return V4L2CompatManager::instance()->mmap(addr, length, prot, flags, + fd, offset); +} + +LIBCAMERA_PUBLIC int munmap(void *addr, size_t length) +{ + return V4L2CompatManager::instance()->munmap(addr, length); +} + +LIBCAMERA_PUBLIC int ioctl(int fd, unsigned long request, ...) +{ + void *arg; + extract_va_arg(void *, arg, request); + + return V4L2CompatManager::instance()->ioctl(fd, request, arg); +} + +} diff --git a/src/v4l2/v4l2_compat_manager.cpp b/src/v4l2/v4l2_compat_manager.cpp new file mode 100644 index 00000000..a3d69374 --- /dev/null +++ b/src/v4l2/v4l2_compat_manager.cpp @@ -0,0 +1,250 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2019, Google Inc. + * + * v4l2_compat_manager.cpp - V4L2 compatibility manager + */ + +#include "v4l2_compat_manager.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "log.h" + +using namespace libcamera; + +LOG_DEFINE_CATEGORY(V4L2Compat) + +V4L2CompatManager::V4L2CompatManager() + : cm_(nullptr), initialized_(false) +{ + openat_func_ = (openat_func_t)dlsym(RTLD_NEXT, "openat"); + dup_func_ = (dup_func_t )dlsym(RTLD_NEXT, "dup"); + close_func_ = (close_func_t )dlsym(RTLD_NEXT, "close"); + ioctl_func_ = (ioctl_func_t )dlsym(RTLD_NEXT, "ioctl"); + mmap_func_ = (mmap_func_t )dlsym(RTLD_NEXT, "mmap"); + munmap_func_ = (munmap_func_t)dlsym(RTLD_NEXT, "munmap"); +} + +V4L2CompatManager::~V4L2CompatManager() +{ + devices_.clear(); + mmaps_.clear(); + + if (isRunning()) { + exit(0); + /* \todo Wait with a timeout, just in case. */ + wait(); + } +} + +int V4L2CompatManager::init() +{ + start(); + + MutexLocker locker(mutex_); + cv_.wait(locker, [&] { return initialized_; }); + + return 0; +} + +void V4L2CompatManager::run() +{ + cm_ = new CameraManager(); + + int ret = cm_->start(); + if (ret) { + LOG(V4L2Compat, Error) << "Failed to start camera manager: " + << strerror(-ret); + return; + } + + LOG(V4L2Compat, Debug) << "Started camera manager"; + + /* + * For each Camera registered in the system, a V4L2CameraProxy gets + * created here to wrap a camera device. + */ + unsigned int index = 0; + for (auto &camera : cm_->cameras()) { + V4L2CameraProxy *proxy = new V4L2CameraProxy(index, camera); + proxies_.emplace_back(proxy); + ++index; + } + + /* + * libcamera has been initialized. Unlock the init() caller as we're + * now ready to handle calls from the framework. + */ + mutex_.lock(); + initialized_ = true; + mutex_.unlock(); + cv_.notify_one(); + + /* Now start processing events and messages. */ + exec(); + + proxies_.clear(); + cm_->stop(); + delete cm_; + cm_ = nullptr; +} + +V4L2CompatManager *V4L2CompatManager::instance() +{ + static V4L2CompatManager instance; + return &instance; +} + +V4L2CameraProxy *V4L2CompatManager::getProxy(int fd) +{ + auto device = devices_.find(fd); + if (device == devices_.end()) + return nullptr; + + return device->second; +} + +int V4L2CompatManager::getCameraIndex(int fd) +{ + struct stat statbuf; + int ret = fstat(fd, &statbuf); + if (ret < 0) + return -1; + + std::shared_ptr target = cm_->get(statbuf.st_rdev); + if (!target) + return -1; + + unsigned int index = 0; + for (auto &camera : cm_->cameras()) { + if (camera == target) + return index; + ++index; + } + + return -1; +} + +int V4L2CompatManager::openat(int dirfd, const char *path, int oflag, mode_t mode) +{ + int fd = openat_func_(dirfd, path, oflag, mode); + if (fd < 0) + return fd; + + struct stat statbuf; + int ret = fstat(fd, &statbuf); + if (ret < 0 || (statbuf.st_mode & S_IFMT) != S_IFCHR || + major(statbuf.st_rdev) != 81) + return fd; + + if (!isRunning()) + init(); + + ret = getCameraIndex(fd); + if (ret < 0) { + LOG(V4L2Compat, Info) << "No camera found for " << path; + return fd; + } + + close_func_(fd); + + unsigned int camera_index = static_cast(ret); + + V4L2CameraProxy *proxy = proxies_[camera_index].get(); + ret = proxy->open(oflag & O_NONBLOCK); + if (ret < 0) + return ret; + + int efd = eventfd(0, oflag & (O_CLOEXEC | O_NONBLOCK)); + if (efd < 0) { + proxy->close(); + return efd; + } + + devices_.emplace(efd, proxy); + + return efd; +} + +int V4L2CompatManager::dup(int oldfd) +{ + int newfd = dup_func_(oldfd); + if (newfd < 0) + return newfd; + + auto device = devices_.find(oldfd); + if (device != devices_.end()) { + V4L2CameraProxy *proxy = device->second; + devices_[newfd] = proxy; + proxy->dup(); + } + + return newfd; +} + +int V4L2CompatManager::close(int fd) +{ + V4L2CameraProxy *proxy = getProxy(fd); + if (proxy) { + proxy->close(); + devices_.erase(fd); + return 0; + } + + return close_func_(fd); +} + +void *V4L2CompatManager::mmap(void *addr, size_t length, int prot, int flags, + int fd, off_t offset) +{ + V4L2CameraProxy *proxy = getProxy(fd); + if (!proxy) + return mmap_func_(addr, length, prot, flags, fd, offset); + + void *map = proxy->mmap(length, prot, flags, offset); + if (map == MAP_FAILED) + return map; + + mmaps_[map] = proxy; + return map; +} + +int V4L2CompatManager::munmap(void *addr, size_t length) +{ + auto device = mmaps_.find(addr); + if (device == mmaps_.end()) + return munmap_func_(addr, length); + + V4L2CameraProxy *proxy = device->second; + + int ret = proxy->munmap(addr, length); + if (ret < 0) + return ret; + + mmaps_.erase(device); + + return 0; +} + +int V4L2CompatManager::ioctl(int fd, unsigned long request, void *arg) +{ + V4L2CameraProxy *proxy = getProxy(fd); + if (!proxy) + return ioctl_func_(fd, request, arg); + + return proxy->ioctl(request, arg); +} diff --git a/src/v4l2/v4l2_compat_manager.h b/src/v4l2/v4l2_compat_manager.h new file mode 100644 index 00000000..dd9d3210 --- /dev/null +++ b/src/v4l2/v4l2_compat_manager.h @@ -0,0 +1,77 @@ +/* SPDX-License-Identifier: GPL-2.0-or-later */ +/* + * Copyright (C) 2019, Google Inc. + * + * v4l2_compat_manager.h - V4L2 compatibility manager + */ + +#ifndef __V4L2_COMPAT_MANAGER_H__ +#define __V4L2_COMPAT_MANAGER_H__ + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "thread.h" +#include "v4l2_camera_proxy.h" + +using namespace libcamera; + +class V4L2CompatManager : public Thread +{ +public: + static V4L2CompatManager *instance(); + + int init(); + + V4L2CameraProxy *getProxy(int fd); + + int openat(int dirfd, const char *path, int oflag, mode_t mode); + + int dup(int oldfd); + int close(int fd); + void *mmap(void *addr, size_t length, int prot, int flags, + int fd, off_t offset); + int munmap(void *addr, size_t length); + int ioctl(int fd, unsigned long request, void *arg); + +private: + V4L2CompatManager(); + ~V4L2CompatManager(); + + void run() override; + int getCameraIndex(int fd); + + typedef int (*openat_func_t)(int dirfd, const char *path, int oflag, ...); + typedef int (*dup_func_t)(int oldfd); + typedef int (*close_func_t)(int fd); + typedef int (*ioctl_func_t)(int fd, unsigned long request, ...); + typedef void *(*mmap_func_t)(void *addr, size_t length, int prot, + int flags, int fd, off_t offset); + typedef int (*munmap_func_t)(void *addr, size_t length); + + openat_func_t openat_func_; + dup_func_t dup_func_; + close_func_t close_func_; + ioctl_func_t ioctl_func_; + mmap_func_t mmap_func_; + munmap_func_t munmap_func_; + + CameraManager *cm_; + + std::mutex mutex_; + std::condition_variable cv_; + bool initialized_; + + std::vector> proxies_; + std::map devices_; + std::map mmaps_; +}; + +#endif /* __V4L2_COMPAT_MANAGER_H__ */ -- cgit v1.2.1