Interface for the Raspberry Pi Broadcom ISP (/dev/video12) via V4L2 M2M

libpsresample.cpp

#include <stdio.h>
#include <stdlib.h>
#include <memory>
#include <cstring>
#include <vector>
#include <string.h>
#include <assert.h>

#include <getopt.h>             /* getopt_long() */
#include <fcntl.h>              /* low-level i/o */
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include <linux/videodev2.h>

#include <stdexcept>
#include <iostream>

#include "libpsvideo.h"
#include "libpsresample.h"

using namespace std;

struct m2m_buffer {
	void* start;
	size_t length;
	struct v4l2_buffer buffer;
	struct v4l2_plane plane;
};

static int fd = -1;
static struct m2m_buffer output;	// (actually the input)
static struct m2m_buffer capture;	// (where the result goes)
static uint32_t sourceX, sourceY, targetX, targetY, source_format;
static std::string device_name;

static void open_device(const char *dev_name)
{
	device_name = std::string(dev_name);

	struct stat st;
	if (-1 == stat(dev_name, &st))
		throw std::runtime_error(string_format("Cannot identify m2m device '%s': %d, %s", dev_name, errno, strerror(errno)));

	if (!S_ISCHR(st.st_mode))
		throw std::runtime_error(string_format("%s is not an m2m device", dev_name));

	fd = open(dev_name, O_RDWR, 0);
	if (-1 == fd)
		throw std::runtime_error(string_format("Cannot open m2m device '%s': %d, %s", dev_name, errno, strerror(errno)));
}

static void check_capabilities(const char *dev_name)
{
	struct v4l2_capability cap;
	if (-1 == xioctl(fd, VIDIOC_QUERYCAP, &cap)) {
		if (EINVAL == errno)
			throw std::runtime_error(string_format("'%s' is not a V4L2 device.", dev_name));
		else
			throw std::runtime_error(string_format("QUERYCAP error: %d, %s", errno, strerror(errno)));
	}

	if (!(cap.capabilities & V4L2_CAP_VIDEO_M2M_MPLANE))
		throw std::runtime_error(string_format("%s is not a multiplane M2M device.", dev_name)); 

	if (!(cap.capabilities & V4L2_CAP_STREAMING))
		throw std::runtime_error(string_format("%s does not support streaming i/o.", dev_name));

	if (!(cap.capabilities & V4L2_CAP_EXT_PIX_FORMAT))
		throw std::runtime_error(string_format("%s does not support extended pix format.", dev_name));
}

static void init_format()
{
	struct v4l2_format fmt;
	struct v4l2_pix_format_mplane *mp = &fmt.fmt.pix_mp;
	CLEAR(fmt);

	fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	if (-1 == xioctl(fd, VIDIOC_G_FMT, &fmt))
		throw std::runtime_error(string_format("VIDIOC_G_FMT error: %d, %s", errno, strerror(errno)));
	mp->width = sourceX;
	mp->height = sourceY;
	fmt.fmt.pix.pixelformat = source_format;
	if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
		throw std::runtime_error(string_format("VIDIOC_S_FMT error: %d, %s", errno, strerror(errno)));
	if (mp->width != sourceX || mp->height != sourceY)
		throw std::runtime_error(string_format("Resampler: OUTPUT_MPLANE wants %dx%d, rather than %dx%d.", mp->width, mp->height, sourceX, sourceY));

	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	if (-1 == xioctl(fd, VIDIOC_G_FMT, &fmt))
		throw std::runtime_error(string_format("VIDIOC_G_FMT error: %d, %s", errno, strerror(errno)));
	mp->width = targetX;
	mp->height = targetY;
	fmt.fmt.pix.pixelformat = TARGET_PIXEL_FORMAT;
	if (-1 == xioctl(fd, VIDIOC_S_FMT, &fmt))
		throw std::runtime_error(string_format("VIDIOC_S_FMT error: %d, %s", errno, strerror(errno)));
	if (mp->width != targetX || mp->height != targetY)
		throw std::runtime_error(string_format("Resampler: CAPTURE_MPLANE wants %dx%d, rather than %dx%d.", mp->width, mp->height, sourceX, sourceY));
}

static void prepare_buffer(m2m_buffer &buffer, uint32_t type) {
	struct v4l2_requestbuffers req;
	CLEAR(req);
	req.count = 1;
	req.memory = V4L2_MEMORY_MMAP;
	req.type = type;
	if (-1 == xioctl(fd, VIDIOC_REQBUFS, &req))
		throw std::runtime_error("Device does not support memory mapping."); 

	if (req.count < 1)
		throw std::runtime_error("Insufficient buffer memory."); 

	buffer.buffer.type = type;
	buffer.buffer.memory = V4L2_MEMORY_MMAP;
	buffer.buffer.index = 0;
	buffer.buffer.length = 1;
	buffer.buffer.m.planes = &buffer.plane;

	if (-1 == xioctl(fd, VIDIOC_QUERYBUF, &buffer.buffer))
		throw std::runtime_error(string_format("VIDIOC_QUERYBUF error: %d, %s", errno, strerror(errno)));

	buffer.length = buffer.buffer.m.planes[0].length;
	buffer.start =
		mmap(NULL /* start anywhere */,
			buffer.length,
			PROT_READ | PROT_WRITE,
			MAP_SHARED,
			fd, 
			buffer.buffer.m.planes[0].m.mem_offset
		);
	if (MAP_FAILED == buffer.start)
		throw std::runtime_error("mmap failed.");
}

static void init_mmap()
{
	prepare_buffer(output, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE);

	prepare_buffer(capture, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE);
	auto expected = TARGET_BYTES_PER_PIXEL * targetX * targetY;
	if (expected != capture.length)
		throw std::runtime_error(string_format("Resampler: expecting target buffer of size %d doesn't match actual of %d.", expected, capture.length));
}

static void queue_buffers() {
	queue_buf(fd, &output.buffer);
	queue_buf(fd, &capture.buffer);
}

static void start_streaming() {
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
		throw std::runtime_error(string_format("VIDIOC_STREAMON error: %d, %s", errno, strerror(errno)));

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	if (-1 == xioctl(fd, VIDIOC_STREAMON, &type))
		throw std::runtime_error(string_format("VIDIOC_STREAMON error: %d, %s", errno, strerror(errno)));
}

static void stop_streaming() {
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
		throw std::runtime_error(string_format("VIDIOC_STREAMOFF error: %d, %s", errno, strerror(errno)));

	type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	if (-1 == xioctl(fd, VIDIOC_STREAMOFF, &type))
		throw std::runtime_error(string_format("VIDIOC_STREAMOFF error: %d, %s", errno, strerror(errno)));
}

void resample_open(const char *dev_name, uint32_t input_format, uint32_t inputX, uint32_t inputY, uint32_t outputX, uint32_t outputY) {
	sourceX = inputX; sourceY = inputY; targetX = outputX; targetY = outputY; source_format = input_format;
	open_device(dev_name);
	check_capabilities(dev_name);
	init_format();
	init_mmap();
	queue_buffers();
	start_streaming();
}

static void write_into_source(std::function<void(void *, uint32_t)> copyCallback) {
	dequeue_buf(fd, &output.buffer);
	copyCallback(output.start, output.length);
	queue_buf(fd, &output.buffer);
}

static void read_from_target(void *dest, uint32_t destSize) {
	dequeue_buf(fd, &capture.buffer);
	auto bytes = capture.buffer.m.planes[0].bytesused;
	if (bytes != destSize)
		throw std::invalid_argument(string_format("Resample: destSize is %d rather than expected %d.", destSize, bytes));
	assert(bytes == targetX * targetY * TARGET_BYTES_PER_PIXEL);	// WARNING: hardware aligns rows to 16, this check ensures that the image is fully packed; this will (intentionally) fail unless the X is a multiple of 16
	std::memcpy(dest, capture.start, bytes);
	queue_buf(fd, &capture.buffer);
}

void resample_run(std::function<void(void *, uint32_t)> copyCallback, void *dest, uint32_t destSize) {
	// HACK: seems to be one frame off.
	write_into_source(copyCallback);
	// dummy read
	dequeue_buf(fd, &capture.buffer);
	queue_buf(fd, &capture.buffer);

	// requeue same
	dequeue_buf(fd, &output.buffer);
	queue_buf(fd, &output.buffer);
	read_from_target(dest, destSize);
}

static void uninit_device() {
	if (-1 == munmap(output.start, output.length))
		throw std::runtime_error("munmap failed.");

	if (-1 == munmap(capture.start, capture.length))
		throw std::runtime_error("munmap failed.");
}

static void close_device() {
	if (-1 == close(fd))
		throw std::runtime_error("Close() error.");

	fd = -1;
}

void resample_close() {
	if (fd == -1)
		return;

	stop_streaming();
	uninit_device();
	close_device();
}

libpsresample.h

#pragma once

#include <functional>

void resample_open(const char *dev_name, uint32_t input_format, uint32_t inputX, uint32_t inputY, uint32_t outputX, uint32_t outputY);
void resample_run(std::function<void(void *, uint32_t)> copyCallback, void *dest, uint32_t destSize);
void resample_close();

libpsvideo.cpp

#include "libpsvideo.h"
#include <sys/ioctl.h>

std::string last_error;

int xioctl(int fh, int request, void* arg)
{
	int r;
	do {
		r = ioctl(fh, request, arg);
	} while (-1 == r && EINTR == errno);
	return r;
}

void dequeue_buf(int fh, v4l2_buffer *buf) {
	while (true) {
		if (-1 == xioctl(fh, VIDIOC_DQBUF, buf)) {
			if (errno == EAGAIN)
				continue;
			throw std::runtime_error(string_format("VIDIOC_DQBUF error: %d, %s", errno, strerror(errno)));
		}
		else
			break;
	}
}

void queue_buf(int fh, v4l2_buffer *buf) {
	if (-1 == xioctl(fh, VIDIOC_QBUF, buf))
		throw std::runtime_error(string_format("VIDIOC_QBUF error: %d, %s", errno, strerror(errno)));
}

libpsvideo.h

#pragma once

//#include <sys/ioctl.h>
#include <errno.h>
#include <string>
#include <cstring>
#include <stdexcept>
#include <stdint.h>
#include <memory>
#include <linux/videodev2.h>

using namespace std;

struct VideoBuffer {
	void* start;
	size_t length;
};

static const int TARGET_BYTES_PER_PIXEL = 3;	// RGB24
static const int TARGET_PIXEL_FORMAT = V4L2_PIX_FMT_RGB24;

#define CLEAR(x) std::memset(&(x), 0, sizeof(x))

extern std::string last_error;

template<typename ... Args>
std::string string_format(const std::string& format, Args ... args)
{
    int size_s = std::snprintf( nullptr, 0, format.c_str(), args ... ) + 1; // Extra space for '\0'
    if( size_s <= 0 ){ throw std::runtime_error( "Error during formatting." ); }
    auto size = static_cast<size_t>( size_s );
    std::unique_ptr<char[]> buf( new char[ size ] );
    std::snprintf( buf.get(), size, format.c_str(), args ... );
    return std::string( buf.get(), buf.get() + size - 1 ); // We don't want the '\0' inside
}

int xioctl(int fh, int request, void* arg);
void dequeue_buf(int fh, v4l2_buffer *buf);
void queue_buf(int fh, v4l2_buffer *buf);