Test program demonstrating the V4L2 request API on a capture device (vivid)

captest.c

/*
 * This program illustrates how the V4L2 request API can be used on a capture
 * device, using the vivid driver. It must be invoked with the path to the vivid
 * capture device as parameter, e.g.
 *
 * $ ./captest /dev/videoX
 *
 * NB_CAPTURES of decreasing brightness are then scheduled on the capture device,
 * then saved to disk as captureX.rgb. The request API is used to confirm that
 * the captures have been performed using the expected brightness, and the
 * captured images can be viewed to further confirm this.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <string.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <poll.h>

#include <linux/videodev2.h>
#include <linux/v4l2-controls.h>
#include <linux/media-request.h>

#define NB_CAPTURES 4
/* desired brightness for ith capture */
#define EXP_BRIGHTNESS(i) (128 - ((128 / NB_CAPTURES) * i))


static void open_device(const char *cap, int *fd)
{
	struct media_request_new new;
	int ret;

	printf("Opening %s\n", cap);
	*fd = open(cap, O_RDWR, 0);
	if (*fd < 0) {
		perror("cannot open capture file");
		exit(EXIT_FAILURE);
	}

	memset(&new, 0, sizeof(new));
	new.flags |= MEDIA_REQUEST_FLAG_TEST;
	ret = ioctl(*fd, VIDIOC_NEW_REQUEST, &new);
	if (ret < 0) {
		printf("Requests not supported by this device! Aborting...\n");
		exit(EXIT_FAILURE);
	}
}

static void set_format(int fd)
{
	struct v4l2_format format;
	int ret;

	memset(&format, 0, sizeof(format));
	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	format.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
	format.fmt.pix.width = 1280;
	format.fmt.pix.height = 720;
	ret = ioctl(fd, VIDIOC_S_FMT, &format);
	if (ret < 0) {
		perror("error while setting format");
		exit(EXIT_FAILURE);
	}
}

static void print_format(int fd)
{
	struct v4l2_format format;
	int ret;

	memset(&format, 0, sizeof(format));
	format.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	ret = ioctl(fd, VIDIOC_G_FMT, &format);
	if (ret < 0) {
		perror("error while getting format");
		exit(EXIT_FAILURE);
	}
	printf("Current format: %ux%u (%d bpl), %08x %08x image size %u\n", format.fmt.pix.width, format.fmt.pix.height, format.fmt.pix.bytesperline, format.fmt.pix.colorspace, format.fmt.pix.pixelformat, format.fmt.pix.sizeimage);
}

static void request_buffers(int fd, unsigned int count)
{
	struct v4l2_requestbuffers requestbuffers;
	int ret;

	memset(&requestbuffers, 0, sizeof(requestbuffers));
	requestbuffers.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	requestbuffers.memory = V4L2_MEMORY_MMAP;
	requestbuffers.count = count;
	ret = ioctl(fd, VIDIOC_REQBUFS, &requestbuffers);
	if (ret < 0) {
		perror("error while requesting buffers");
		exit(EXIT_FAILURE);
	}
	printf("%d buffers allocated\n", requestbuffers.count);
}

static void query_buf(int fd, int index, struct v4l2_buffer *buf)
{
	int ret;

	memset(buf, 0, sizeof(*buf));
	buf->memory = V4L2_MEMORY_MMAP;
	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf->index = index;

	ret = ioctl(fd, VIDIOC_QUERYBUF, buf);
	if (ret < 0) {
		perror("error while querying buffer");
		exit(EXIT_FAILURE);
	}
}

static void set_stream(int fd, int on)
{
	int ret;
	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	ret = ioctl(fd, on ? VIDIOC_STREAMON : VIDIOC_STREAMOFF, &type);
	if (ret < 0) {
		perror("error while setting streaming");
		exit(EXIT_FAILURE);
	}
	printf("Capture stream %s\n", on ? "activated" : "deactivated");
}


static void save_buffer(struct v4l2_buffer *bufferinfo, int fd, const char *fname)
{
	void *buffer;
	int file;
	int ret;

	file = open(fname, O_WRONLY | O_CREAT | O_TRUNC, 0660);
	if (file < 0) {
		perror("cannot open file to save buffer");
		exit(EXIT_FAILURE);
	}

	buffer = mmap(NULL, bufferinfo->length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, bufferinfo->m.offset);
	if (buffer == MAP_FAILED) {
		perror("error while mapping buffer");
		exit(EXIT_FAILURE);
	}

	write(file, buffer, bufferinfo->bytesused);
	close(file);

	ret = munmap(buffer, bufferinfo->length);
	if (ret < 0) {
		perror("error while unmapping buffer");
		exit(EXIT_FAILURE);
	}
}

static void get_brightness(int fd, int req, int *val)
{
	struct v4l2_ext_control ctrl[1];
	struct v4l2_ext_controls ctrls;
	int ret;

	memset(&ctrl, 0, sizeof(ctrl));
	ctrl[0].id = V4L2_CID_BRIGHTNESS;

	memset(&ctrls, 0, sizeof(ctrls));
	ctrls.request_fd = req;
	ctrls.count = 1;
	ctrls.controls = ctrl;

	ret = ioctl(fd, VIDIOC_G_EXT_CTRLS, &ctrls);
	if (ret < 0) {
		perror("error while getting control value");
		exit(EXIT_FAILURE);
	}

	if (val)
		*val = ctrl[0].value;

}

static void set_brightness(int fd, int req, int val)
{
	struct v4l2_ext_control ctrl[1];
	struct v4l2_ext_controls ctrls;
	int ret;

	memset(&ctrl, 0, sizeof(ctrl));
	ctrl[0].id = V4L2_CID_BRIGHTNESS;
	ctrl[0].value = val;

	memset(&ctrls, 0, sizeof(ctrls));
	ctrls.request_fd = req;
	ctrls.count = 1;
	ctrls.controls = ctrl;

	ret = ioctl(fd, VIDIOC_S_EXT_CTRLS, &ctrls);
	if (ret < 0) {
		perror("error while setting control value");
		exit(EXIT_FAILURE);
	}
}


static void qbuf(int fd, int req, struct v4l2_buffer *buf)
{
	int ret;

	buf->request_fd = req;

	ret = ioctl(fd, VIDIOC_QBUF, buf);
	if (ret < 0) {
		perror("error while queuing buffer");
		exit(EXIT_FAILURE);
	}
	printf("%s buffer %d queued\n",
	       buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE ? "Capture" : "Output",
	       buf->index);
}

static void dqbuf(int fd, struct v4l2_buffer *buf)
{
	int ret;

	memset(buf, 0, sizeof(*buf));
	buf->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	buf->memory = V4L2_MEMORY_MMAP;
	ret = ioctl(fd, VIDIOC_DQBUF, buf);
	if (ret < 0) {
		perror("error while dequeuing buffer");
		exit(EXIT_FAILURE);
	}
	printf("%s buffer %d dequeued\n",
	       buf->type == V4L2_BUF_TYPE_VIDEO_CAPTURE ? "Capture" : "Output",
	       buf->index);
}

static void alloc_request(int fd, int *req)
{
	struct media_request_new new;
	int ret;

	memset(&new, 0, sizeof(new));
	ret = ioctl(fd, VIDIOC_NEW_REQUEST, &new);
	if (ret < 0) {
		perror("error while allocating request");
		exit(EXIT_FAILURE);
	}

	*req = new.fd;
}

static void submit_request(int req)
{
	int ret;

	ret = ioctl(req, MEDIA_REQUEST_IOC_SUBMIT, NULL);
	if (ret < 0) {
		perror("error while queueing request");
		exit(EXIT_FAILURE);
	}
}

static void wait_request(int req)
{
	struct pollfd pfd = {
		.events = POLLIN,
		.fd = req,
	};

	if (poll(&pfd, 1, -1) != 1) {
		printf("Error while polling request!\n");
		exit(EXIT_FAILURE);
	}
}

int main(int argc, char *argv[])
{
	struct v4l2_capability cap;
	struct v4l2_buffer bufferinfo[NB_CAPTURES];
	int req[NB_CAPTURES];
	int fd;
	int i, val;
	int ret;

	if (argc != 2) {
		printf("Usage: %s /dev/video_device\n", argv[0]);
		return 0;
	}

	open_device(argv[1], &fd);

	ret = ioctl(fd, VIDIOC_QUERYCAP, &cap);
	if (ret < 0) {
		perror("cannot query caps\n");
		exit(EXIT_FAILURE);
	}
	if ((cap.capabilities & (V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE)) !=
		                (V4L2_CAP_STREAMING | V4L2_CAP_VIDEO_CAPTURE)) {
		printf("error: this program requires a capture device capable of streaming\n");
		exit(EXIT_FAILURE);
	}

	printf("Using first input\n");
	i = 0;
	ret = ioctl(fd, VIDIOC_S_INPUT, &i);
	if (ret < 0) {
		perror("error when setting input");
		exit(EXIT_FAILURE);
	}

	set_format(fd);
	print_format(fd);

	printf("Allocating %d requests\n", NB_CAPTURES);
	for (i = 0; i < NB_CAPTURES; i++)
		alloc_request(fd, &req[i]);

	request_buffers(fd, NB_CAPTURES);
	for (i = 0; i < NB_CAPTURES; i++)
		query_buf(fd, i, &bufferinfo[i]);

	set_stream(fd, 1);

	/* Submit requests for all captures */
	for (i = 0; i < NB_CAPTURES; i++) {
		set_brightness(fd, req[i], EXP_BRIGHTNESS(i));
		printf("%dth request scheduled brightness: %d\n", i+1, EXP_BRIGHTNESS(i));

		qbuf(fd, req[i], &bufferinfo[i]);
		submit_request(req[i]);
	}

	/* Dequeue captures and wait for requests, confirm expected brightness values */
	for (i = 0; i < NB_CAPTURES; i++) {
		struct v4l2_buffer buf;
		char str[32];

		dqbuf(fd, &buf);
		snprintf(str, sizeof(str), "capture%d.rgb", i);
		save_buffer(&buf, fd, str);
		printf("%s written\n", str);

		wait_request(req[i]);
		get_brightness(fd, req[i], &val);
		printf("%dth request brightness: %d, expected: %d\n", i+1, val, EXP_BRIGHTNESS(i));
	}

	set_stream(fd, 0);
	request_buffers(fd, 0);

	printf("Closing requests\n");
	for (i = NB_CAPTURES; i > 0; i--)
		close(req[i - 1]);

	close(fd);

	return 0;
}