d/v4l2: Add new frameserver

This commit is contained in:
Jakob Bornecrantz 2019-07-22 19:21:04 +01:00
parent a544b4b56f
commit 718a81608a
5 changed files with 798 additions and 0 deletions

View file

@ -44,6 +44,7 @@ if(CMAKE_SYSTEM_NAME STREQUAL "Linux")
find_package(udev REQUIRED)
set(BUILD_WITH_LIBUDEV TRUE)
set(BUILD_DRIVER_V4L2 TRUE)
endif()
cmake_dependent_option(BUILD_WITH_WAYLAND "Enable Wayland support" ON "WAYLAND_FOUND" OFF)

View file

@ -70,3 +70,14 @@ if(BUILD_DRIVER_PSVR)
PRIVATE ${HIDAPI_INCLUDE_DIRS}
)
endif()
if(BUILD_DRIVER_V4L2)
set(V4L2_SOURCE_FILES
v4l2/v4l2_driver.c
)
# Use OBJECT to not create a archive, since it just gets in the way.
add_library(drv_v4l2 OBJECT ${V4L2_SOURCE_FILES})
set_property(TARGET drv_v4l2 PROPERTY POSITION_INDEPENDENT_CODE ON)
endif()

View file

@ -0,0 +1,710 @@
// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief V4L2 frameserver implementation
* @author Pete Black <pblack@collabora.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_v4l2
*/
#include "util/u_misc.h"
#include "util/u_debug.h"
#include "util/u_format.h"
#include "xrt/xrt_frameserver.h"
#include "v4l2_interface.h"
#include <stdio.h>
#include <assert.h>
#include <unistd.h>
#include <pthread.h>
#include <linux/videodev2.h>
#include <linux/v4l2-common.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <fcntl.h>
/*
*
* Defines.
*
*/
#define V_SPEW(p, ...) \
do { \
if (p->print_spew) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define V_DEBUG(p, ...) \
do { \
if (p->print_debug) { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} \
} while (false)
#define V_ERROR(p, ...) \
do { \
fprintf(stderr, "%s - ", __func__); \
fprintf(stderr, __VA_ARGS__); \
fprintf(stderr, "\n"); \
} while (false)
DEBUG_GET_ONCE_BOOL_OPTION(v4l2_spew, "V4L2_PRINT_SPEW", false)
DEBUG_GET_ONCE_BOOL_OPTION(v4l2_debug, "V4L2_PRINT_DEBUG", false)
#define NUM_V4L2_BUFFERS 4
/*
*
* Structs
*
*/
/*!
* A single open v4l2 capture device, starts it's own thread and waits on it.
*/
struct v4l2_fs
{
struct xrt_fs base;
int fd;
struct
{
bool extended_format;
bool timeperframe;
} has;
struct
{
bool ps4_cam;
} quirks;
struct
{
void *mem[NUM_V4L2_BUFFERS];
bool mmap;
bool userptr;
} capture;
struct xrt_fs_sink *sink;
pthread_t stream_thread;
struct v4l2_source_descriptor *descriptors;
uint32_t num_descriptors;
uint32_t selected;
struct xrt_fs_capture_parameters capture_params;
bool is_configured;
bool is_running;
bool print_spew;
bool print_debug;
};
/*!
* Streaming thread entrypoint
*/
static void *
v4l2_fs_stream_run(void *ptr);
/*!
* Cast to derived type.
*/
static inline struct v4l2_fs *
v4l2_fs(struct xrt_fs *xfs)
{
return (struct v4l2_fs *)xfs;
}
/*
*
* Misc helper functions
*
*/
static int
v4l2_query_cap_and_validate(struct v4l2_fs *vid)
{
int ret;
/*
* Regular caps.
*/
struct v4l2_capability cap;
ret = ioctl(vid->fd, VIDIOC_QUERYCAP, &cap);
if (ret != 0) {
V_ERROR(vid, "error: Failed to get v4l2 cap.");
return ret;
}
if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
// not a video device
V_ERROR(vid, "error: Is not a capture device.");
return -1;
}
if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
// cannot stream
V_ERROR(vid, "error: Can not stream!");
return -1;
}
if (cap.capabilities & V4L2_CAP_EXT_PIX_FORMAT) {
// need to query for extended format info
vid->has.extended_format = true;
}
/*
* Stream capture caps.
*/
struct v4l2_streamparm stream = {0};
stream.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
ret = ioctl(vid->fd, VIDIOC_G_PARM, &stream);
if (ret != 0) {
V_ERROR(vid, "error: Failed to get v4l2 stream param.");
return ret;
}
if (stream.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
// Does this device support setting the timeperframe interval.
vid->has.timeperframe = true;
} else {
V_DEBUG(vid, "warning: No V4L2_CAP_TIMEPERFRAME");
}
/*
* Find quirks
*/
char *card = (char *)cap.card;
vid->quirks.ps4_cam =
strcmp(card, "USB Camera-OV580: USB Camera-OV") == 0;
// Done
return 0;
}
static int
v4l2_try_userptr(struct v4l2_fs *vid, struct v4l2_requestbuffers *v_bufrequest)
{
v_bufrequest->memory = V4L2_MEMORY_USERPTR;
if (ioctl(vid->fd, VIDIOC_REQBUFS, v_bufrequest) == 0) {
vid->capture.userptr = true;
return 0;
}
V_DEBUG(vid, "info: Driver does not handle userptr buffers.");
return -1;
}
static int
v4l2_try_mmap(struct v4l2_fs *vid, struct v4l2_requestbuffers *v_bufrequest)
{
v_bufrequest->memory = V4L2_MEMORY_MMAP;
if (ioctl(vid->fd, VIDIOC_REQBUFS, v_bufrequest) == 0) {
vid->capture.mmap = true;
return 0;
}
V_DEBUG(vid, "info: Driver does not mmap userptr buffers.");
return -1;
}
static int
v4l2_setup_mmap_buffer(struct v4l2_fs *vid,
int index,
struct v4l2_buffer *v_buf)
{
void *ptr = mmap(0, v_buf->length, PROT_READ, MAP_SHARED, vid->fd,
v_buf->m.offset);
if (ptr == MAP_FAILED) {
V_ERROR(vid, "error: Call to mmap failed!");
return -1;
}
vid->capture.mem[index] = ptr;
return 0;
}
static int
v4l2_setup_userptr_buffer(struct v4l2_fs *vid,
int index,
struct v4l2_buffer *v_buf)
{
// align this to a memory page, v4l2 likes it that way
void *ptr = aligned_alloc(getpagesize(), v_buf->length);
if (ptr == NULL) {
V_ERROR(vid, "error: Could not alloc page-aligned memory!");
return -1;
}
vid->capture.mem[index] = ptr;
v_buf->m.userptr = (intptr_t)ptr;
return 0;
}
/*
*
* Mode adding functions.
*
*/
static void
v4l2_quirk_apply_ps4(struct v4l2_fs *vid, struct v4l2_source_descriptor *desc)
{
desc->offset = 32 + 64;
desc->base.stereo_format = XRT_FS_STEREO_SBS;
switch (desc->stream.width) {
case 3448:
desc->base.width = 1280 * 2;
desc->base.height = 800;
break;
case 1748:
desc->base.width = 640 * 2;
desc->base.height = 400;
break;
case 898:
desc->base.width = 320 * 2;
desc->base.height = 192;
break;
default: break;
}
}
static struct v4l2_source_descriptor *
v4l2_add_descriptor(struct v4l2_fs *vid)
{
uint32_t index = vid->num_descriptors++;
size_t new_size =
vid->num_descriptors * sizeof(struct v4l2_source_descriptor);
vid->descriptors = realloc(vid->descriptors, new_size);
struct v4l2_source_descriptor *desc = &vid->descriptors[index];
memset(desc, 0, sizeof(*desc));
return desc;
}
static void
v4l2_list_modes_interval(struct v4l2_fs *vid,
const struct v4l2_fmtdesc *fmt,
const struct v4l2_frmsizeenum *size,
const struct v4l2_frmivalenum *interval)
{
if (interval->discrete.denominator % interval->discrete.numerator ==
0) {
int fps = interval->discrete.denominator /
interval->discrete.numerator;
V_DEBUG(vid, "#%i %dx%d@%i", vid->num_descriptors,
interval->width, interval->height, fps);
} else {
double fps = (double)interval->discrete.denominator /
(double)interval->discrete.numerator;
V_DEBUG(vid, "#%i %dx%d@%f", vid->num_descriptors,
interval->width, interval->height, fps);
}
}
static void
v4l2_list_modes_size(struct v4l2_fs *vid,
const struct v4l2_fmtdesc *fmt,
const struct v4l2_frmsizeenum *size)
{
if (size->type != V4L2_FRMSIZE_TYPE_DISCRETE) {
V_DEBUG(vid, "warning: Skipping non discrete frame size.");
return;
}
struct v4l2_frmivalenum interval;
U_ZERO(&interval);
interval.pixel_format = size->pixel_format;
interval.width = size->discrete.width;
interval.height = size->discrete.height;
// Since we don't keep track of the interval
// we only make sure there is at least one.
while (ioctl(vid->fd, VIDIOC_ENUM_FRAMEINTERVALS, &interval) == 0) {
v4l2_list_modes_interval(vid, fmt, size, &interval);
interval.index++;
}
// We didn't find any frame intervals.
if (interval.index == 0) {
return;
}
enum xrt_format format = 0;
switch (interval.pixel_format) {
case V4L2_PIX_FMT_YUYV: format = XRT_FORMAT_YUV422; break;
case V4L2_PIX_FMT_MJPEG: format = XRT_FORMAT_MJPEG; break;
default: V_ERROR(vid, "error: Format not supported."); return;
}
// Allocate new descriptor.
struct v4l2_source_descriptor *desc = v4l2_add_descriptor(vid);
// Fill out the stream variables.
desc->stream.width = interval.width;
desc->stream.height = interval.height;
desc->stream.format = interval.pixel_format;
snprintf(desc->format_name, sizeof(desc->format_name), "%s",
fmt->description);
if (u_format_is_blocks(format)) {
u_format_size_for_dimensions(
format, interval.width, interval.height,
&desc->stream.stride, &desc->stream.size);
}
// Fill out the out sink variables.
desc->base.stereo_format = XRT_FS_STEREO_NONE;
desc->base.format = format;
desc->base.width = desc->stream.width;
desc->base.height = desc->stream.height;
/*
* Apply any quirks to the modes.
*/
if (vid->quirks.ps4_cam) {
v4l2_quirk_apply_ps4(vid, desc);
}
}
static void
v4l2_list_modes_fmt(struct v4l2_fs *vid, const struct v4l2_fmtdesc *fmt)
{
V_DEBUG(vid, "format: %s %08x %d", fmt->description, fmt->pixelformat,
fmt->type);
switch (fmt->pixelformat) {
case V4L2_PIX_FMT_YUYV: break;
case V4L2_PIX_FMT_MJPEG: break;
default:
V_ERROR(vid, "error: Unknown pixelformat '%s'",
fmt->description);
return;
}
struct v4l2_frmsizeenum size = {0};
size.pixel_format = fmt->pixelformat;
while (ioctl(vid->fd, VIDIOC_ENUM_FRAMESIZES, &size) == 0) {
v4l2_list_modes_size(vid, fmt, &size);
size.index++;
}
}
static void
v4l2_list_modes(struct v4l2_fs *vid)
{
struct v4l2_fmtdesc desc;
U_ZERO(&desc);
desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
while (ioctl(vid->fd, VIDIOC_ENUM_FMT, &desc) == 0) {
v4l2_list_modes_fmt(vid, &desc);
desc.index++;
}
}
/*
*
* Exported functions.
*
*/
static bool
v4l2_fs_enumerate_modes(struct xrt_fs *xfs,
struct xrt_fs_mode **out_modes,
uint32_t *out_count)
{
struct v4l2_fs *vid = v4l2_fs(xfs);
if (vid->num_descriptors == 0) {
return false;
}
struct xrt_fs_mode *modes =
U_TYPED_ARRAY_CALLOC(struct xrt_fs_mode, vid->num_descriptors);
if (modes == NULL) {
return false;
}
for (uint32_t i = 0; i < vid->num_descriptors; i++) {
modes[i] = vid->descriptors[i].base;
}
*out_modes = modes;
*out_count = vid->num_descriptors;
return true;
}
static bool
v4l2_fs_configure_capture(struct xrt_fs *xfs,
struct xrt_fs_capture_parameters *cp)
{
// struct v4l2_fs *vid = v4l2_fs(xfs);
//! @todo
return false;
}
static bool
v4l2_fs_stream_start(struct xrt_fs *xfs, uint32_t descriptor_index)
{
struct v4l2_fs *vid = v4l2_fs(xfs);
if (descriptor_index >= vid->num_descriptors) {
V_ERROR(vid, "error Invalid descriptor_index (%i >= %i)",
descriptor_index, vid->num_descriptors);
return false;
}
vid->selected = descriptor_index;
vid->is_running = true;
if (pthread_create(&vid->stream_thread, NULL, v4l2_fs_stream_run,
xfs)) {
vid->is_running = false;
V_ERROR(vid, "error: Could not create thread");
return false;
}
V_SPEW(vid, "info: Started!\n");
// we're off to the races!
return true;
}
static bool
v4l2_fs_stream_stop(struct xrt_fs *xfs)
{
struct v4l2_fs *vid = v4l2_fs(xfs);
if (!vid->is_running) {
return true;
}
vid->is_running = false;
pthread_join(vid->stream_thread, NULL);
return true;
}
static bool
v4l2_fs_is_running(struct xrt_fs *xfs)
{
struct v4l2_fs *vid = v4l2_fs(xfs);
return vid->is_running;
}
static void
v4l2_fs_destroy(struct xrt_fs *xfs)
{
struct v4l2_fs *vid = v4l2_fs(xfs);
// Make sure that the stream is stopped.
v4l2_fs_stream_stop(xfs);
if (vid->descriptors != NULL) {
free(vid->descriptors);
vid->descriptors = NULL;
vid->num_descriptors = 0;
}
vid->capture.mmap = false;
if (vid->capture.userptr) {
vid->capture.userptr = false;
for (uint32_t i = 0; i < NUM_V4L2_BUFFERS; i++) {
free(vid->capture.mem[i]);
vid->capture.mem[i] = NULL;
}
}
if (vid->fd >= 0) {
close(vid->fd);
vid->fd = -1;
}
free(vid);
}
struct xrt_fs *
v4l2_fs_create(const char *path, struct xrt_fs_sink *sink)
{
struct v4l2_fs *vid = U_TYPED_CALLOC(struct v4l2_fs);
vid->base.enumerate_modes = v4l2_fs_enumerate_modes;
vid->base.configure_capture = v4l2_fs_configure_capture;
vid->base.stream_start = v4l2_fs_stream_start;
vid->base.stream_stop = v4l2_fs_stream_stop;
vid->base.is_running = v4l2_fs_is_running;
vid->base.destroy = v4l2_fs_destroy;
vid->print_spew = debug_get_bool_option_v4l2_spew();
vid->print_debug = debug_get_bool_option_v4l2_debug();
vid->sink = sink;
vid->fd = -1;
int fd = open(path, O_RDWR, 0);
if (fd < 0) {
V_ERROR(vid, "Can not open '%s'\n", path);
free(vid);
return NULL;
}
vid->fd = fd;
int ret = v4l2_query_cap_and_validate(vid);
if (ret != 0) {
v4l2_fs_destroy(&vid->base);
vid = NULL;
return NULL;
}
v4l2_list_modes(vid);
return &(vid->base);
}
void *
v4l2_fs_stream_run(void *ptr)
{
struct xrt_fs *xfs = (struct xrt_fs *)ptr;
struct v4l2_fs *vid = v4l2_fs(xfs);
V_DEBUG(vid, "info: Thread enter!");
if (vid->fd == -1) {
V_ERROR(vid, "error: Device not opened!");
return NULL;
}
// set up our capture format
struct v4l2_source_descriptor *desc = &vid->descriptors[vid->selected];
struct v4l2_format v_format;
U_ZERO(&v_format);
v_format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v_format.fmt.pix.width = desc->stream.width;
v_format.fmt.pix.height = desc->stream.height;
v_format.fmt.pix.pixelformat = desc->stream.format;
v_format.fmt.pix.field = V4L2_FIELD_ANY;
if (vid->has.extended_format) {
v_format.fmt.pix.priv = V4L2_PIX_FMT_PRIV_MAGIC;
}
if (ioctl(vid->fd, VIDIOC_S_FMT, &v_format) < 0) {
V_ERROR(vid, "could not set up format\n");
return NULL;
}
// set up our buffers - prefer userptr (client alloc) vs mmap (kernel
// alloc)
// TODO: using buffer caps may be better than 'fallthrough to mmap'
struct v4l2_requestbuffers v_bufrequest;
U_ZERO(&v_bufrequest);
v_bufrequest.count = NUM_V4L2_BUFFERS;
v_bufrequest.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (v4l2_try_userptr(vid, &v_bufrequest) != 0 &&
v4l2_try_mmap(vid, &v_bufrequest) != 0) {
V_ERROR(vid, "error: Driver does not support mmap or userptr.");
return NULL;
}
// set up our buffers
struct v4l2_buffer v_buf = {0};
for (uint32_t i = 0; i < NUM_V4L2_BUFFERS; i++) {
v_buf.index = i;
v_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
v_buf.memory = v_bufrequest.memory;
if (ioctl(vid->fd, VIDIOC_QUERYBUF, &v_buf) < 0) {
V_ERROR(vid, "error: Could not query buffers!");
return NULL;
}
if (vid->capture.userptr &&
v4l2_setup_userptr_buffer(vid, i, &v_buf) != 0) {
return NULL;
}
if (vid->capture.mmap &&
v4l2_setup_mmap_buffer(vid, i, &v_buf) != 0) {
return NULL;
}
// Silence valgrind.
memset(vid->capture.mem[i], 0, v_buf.length);
// Queue this buffer
if (ioctl(vid->fd, VIDIOC_QBUF, &v_buf) < 0) {
V_ERROR(vid, "error: queueing buffer failed!");
return NULL;
}
}
int start_capture = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (ioctl(vid->fd, VIDIOC_STREAMON, &start_capture) < 0) {
V_ERROR(vid, "error: Could not start capture!");
return NULL;
}
struct xrt_fs_frame f = {0};
while (vid->is_running) {
if (ioctl(vid->fd, VIDIOC_DQBUF, &v_buf) < 0) {
V_ERROR(vid, "error: Dequeue failed!");
vid->is_running = false;
break;
} else {
V_SPEW(vid, "Got frame %i", v_buf.index);
}
uint8_t *data = vid->capture.mem[v_buf.index];
//! @todo Sequense number and timestamp.
f.width = desc->base.width;
f.height = desc->base.height;
f.format = desc->base.format;
f.stereo_format = desc->base.stereo_format;
f.data = data + desc->offset;
f.stride = desc->stream.stride;
f.size = v_buf.bytesused - desc->offset;
f.source_id = vid->base.source_id;
vid->sink->push_frame(vid->sink, &f);
if (ioctl(vid->fd, VIDIOC_QBUF, &v_buf) < 0) {
V_ERROR(vid, "error: Requeue failed!");
vid->is_running = false;
break;
}
}
V_DEBUG(vid, "info: Thread leave!");
return NULL;
}

View file

@ -0,0 +1,71 @@
// Copyright 2019, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Header
* @author Pete Black <pblack@collabora.com>
* @author Jakob Bornecrantz <jakob@collabora.com>
* @ingroup drv_v4l2
*/
#pragma once
#include "xrt/xrt_frameserver.h"
#ifdef __cplusplus
extern "C" {
#endif
/*!
* @defgroup drv_v4l2 V4L2 frameserver driver
* @ingroup drv
*
* @brief Frameserver using the Video 4 Linux 2 framework.
*/
/*!
* Descriptor of a v4l2 source.
*
* @ingroup drv_v4l2
*/
struct v4l2_source_descriptor
{
struct xrt_fs_mode base;
char format_name[32];
struct
{
uint32_t width;
uint32_t height;
uint32_t format;
uint8_t extended_format;
size_t size;
size_t stride;
} stream;
/*!
* Offset from start off frame to start of pixels.
*
* Aka crop_scanline_bytes_start.
*
* Special case for ps4 camera
*/
size_t offset;
uint32_t rate;
};
/*!
* Create a v4l2 frameserver
*
* @ingroup drv_v4l2
*/
struct xrt_fs*
v4l2_fs_create(const char* device, struct xrt_fs_sink* q);
#ifdef __cplusplus
}
#endif

View file

@ -41,6 +41,11 @@ if(BUILD_DRIVER_PSVR)
list(APPEND DRIVER_LIBRARIES ${HIDAPI_LIBRARIES})
endif()
if(BUILD_DRIVER_V4L2)
set(XRT_BUILD_V4L2 TRUE)
list(APPEND DRIVER_OBJECTS $<TARGET_OBJECTS:drv_v4l2>)
endif()
configure_file(targets_enabled_drivers.h.cmake_in ${CMAKE_CURRENT_BINARY_DIR}/targets_enabled_drivers.h)
include_directories(${CMAKE_CURRENT_BINARY_DIR})