V4L2 video encoder

enc.c

#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <linux/videodev2.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/eventfd.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <poll.h>

#define xioctl(a, b, c) do { \
	if (ioctl(a, b, c) < 0) { \
		fprintf(stderr, "%s:%i: IOCTL %s: %s\n", __FILE__, __LINE__, #b, strerror(errno)); \
		abort(); \
	} \
} while (0)

struct mmbuffer {
	void *start;
	size_t length;
	int ready;
};

int main() {
	int width = 640;
	int height = 480;
	int fd = -1;

	struct v4l2_capability cap;
	for (int id = 0; id < 16; ++id) {
		char pathbuf[64];
		snprintf(pathbuf, sizeof(pathbuf), "/dev/video%d", id);
		int tfd = open(pathbuf, O_RDWR);
		if (tfd < 0) {
			continue;
		}

		memset(&cap, 0, sizeof(cap));
		if (ioctl(tfd, VIDIOC_QUERYCAP, &cap) < 0) {
			close(tfd);
			continue;
		}

		if (strcmp((const char *)cap.card, "Qualcomm Venus video encoder") == 0) {
			fprintf(stderr, "Found %s (%s, fd %i)\n", cap.card, pathbuf, tfd);
			fd = tfd;
			break;
		}
	}

	if (fd < 0) {
		fprintf(stderr, "Found no encoder\n");
		return 1;
	}

	// 1. Set the coded format on the CAPTURE queue via VIDIOC_S_FMT().
	struct v4l2_format fmt;
	memset(&fmt, 0, sizeof(fmt));
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_H264;
	fmt.fmt.pix_mp.num_planes = 1;
	fmt.fmt.pix_mp.width = width;
	fmt.fmt.pix_mp.height = height;
	fmt.fmt.pix_mp.plane_fmt[0].sizeimage = 1024 * 1024;
	xioctl(fd, VIDIOC_S_FMT, &fmt);

	// 2. Optional. Enumerate supported OUTPUT formats (raw formats for source) for the selected
	// coded format via VIDIOC_ENUM_FMT().
	struct v4l2_fmtdesc fmtdesc;
	memset(&fmtdesc, 0, sizeof(fmtdesc));
	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
		char fcc[4];
		memcpy(fcc, &fmtdesc.pixelformat, 4);
		fprintf(stderr, "Output format %i: %c%c%c%c: %s\n", fmtdesc.index, fcc[0], fcc[1], fcc[2], fcc[3], fmtdesc.description);
		fmtdesc.index += 1;
	}

	// Let's do the same with CAPTURE
	memset(&fmtdesc, 0, sizeof(fmtdesc));
	fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	while (ioctl(fd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
		char fcc[4];
		memcpy(fcc, &fmtdesc.pixelformat, 4);
		fprintf(stderr, "Capture format %i: %c%c%c%c: %s\n", fmtdesc.index, fcc[0], fcc[1], fcc[2], fcc[3], fmtdesc.description);
		fmtdesc.index += 1;
	}

	// 3. Set the raw source format on the OUTPUT queue via VIDIOC_S_FMT().
	memset(&fmt, 0, sizeof(fmt));
	fmt.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	fmt.fmt.pix_mp.pixelformat = V4L2_PIX_FMT_NV12;
	fmt.fmt.pix_mp.width = width;
	fmt.fmt.pix_mp.height = height;
	xioctl(fd, VIDIOC_S_FMT, &fmt);

	// 4. Set the raw frame interval on the OUTPUT queue via VIDIOC_S_PARM(). This also sets the
	// coded frame interval on the CAPTURE queue to the same value.
	struct v4l2_streamparm parm;
	memset(&parm, 0, sizeof(parm));
	parm.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	parm.parm.output.timeperframe.numerator = 1;
	parm.parm.output.timeperframe.denominator = 30;
	xioctl(fd, VIDIOC_S_PARM, &parm);

	// 5. Optional. Set the coded frame interval on the CAPTURE queue via VIDIOC_S_PARM().
	memset(&parm, 0, sizeof(parm));
	parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	parm.parm.capture.timeperframe.numerator = 1;
	parm.parm.capture.timeperframe.denominator = 30;
	xioctl(fd, VIDIOC_S_PARM, &parm);

	// 6. Optional. Set the visible resolution for the stream metadata via VIDIOC_S_SELECTION() on
	// the OUTPUT queue if it is desired to be different than the full OUTPUT resolution.

	// 7. Allocate buffers for both OUTPUT and CAPTURE via VIDIOC_REQBUFS().
	// This may be performed in any order.

	struct mmbuffer *captureBufs = NULL;
	size_t captureBufCount;
	{ // CAPTURE
		struct v4l2_requestbuffers reqbufs;
		memset(&reqbufs, 0, sizeof(reqbufs));
		reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		reqbufs.memory = V4L2_MEMORY_MMAP;
		reqbufs.count = 4;
		xioctl(fd, VIDIOC_REQBUFS, &reqbufs);

		captureBufCount = reqbufs.count;
		captureBufs = (struct mmbuffer *)malloc(captureBufCount * sizeof(*captureBufs));
		for (size_t i = 0; i < captureBufCount; ++i) {
			struct v4l2_buffer buffer;
			memset(&buffer, 0, sizeof(buffer));
			struct v4l2_plane plane;
			memset(&plane, 0, sizeof(plane));
			buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
			buffer.length = 1;
			buffer.m.planes = &plane;
			buffer.memory = V4L2_MEMORY_MMAP;
			buffer.index = i;
			xioctl(fd, VIDIOC_QUERYBUF, &buffer);

			captureBufs[i].ready = 1;
			captureBufs[i].start = mmap(NULL, plane.length, PROT_READ | PROT_WRITE, MAP_SHARED,
				fd, plane.m.mem_offset);
			captureBufs[i].length = plane.length;
			if (captureBufs[i].start == MAP_FAILED) {
				fprintf(stderr, "mmap: %s\n", strerror(errno));
				return 1;
			}

			fprintf(stderr, "Mapped buffer %zi: %p, %i\n", i, captureBufs[i].start, plane.length);
		}
	}

	struct mmbuffer *outputBufs = NULL;
	size_t outputBufCount;
	{ // OUTPUT
		struct v4l2_requestbuffers reqbufs;
		memset(&reqbufs, 0, sizeof(reqbufs));
		reqbufs.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
		reqbufs.memory = V4L2_MEMORY_MMAP;
		reqbufs.count = 4;
		xioctl(fd, VIDIOC_REQBUFS, &reqbufs);

		outputBufCount = reqbufs.count;
		outputBufs = (struct mmbuffer *)malloc(outputBufCount * sizeof(*captureBufs));
		for (size_t i = 0; i < outputBufCount; ++i) {
			struct v4l2_buffer buffer;
			memset(&buffer, 0, sizeof(buffer));
			struct v4l2_plane plane;
			memset(&plane, 0, sizeof(plane));
			buffer.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
			buffer.length = 1;
			buffer.m.planes = &plane;
			buffer.memory = V4L2_MEMORY_MMAP;
			buffer.index = i;
			xioctl(fd, VIDIOC_QUERYBUF, &buffer);

			outputBufs[i].ready = 1;
			outputBufs[i].start = mmap(NULL, plane.length, PROT_READ | PROT_WRITE, MAP_SHARED,
				fd, plane.m.mem_offset);
			outputBufs[i].length = plane.length;
			if (outputBufs[i].start == MAP_FAILED) {
				fprintf(stderr, "mmap: %s\n", strerror(errno));
				return 1;
			}

			fprintf(stderr, "Mapped buffer %zi: %p, %i\n", i, outputBufs[i].start, plane.length);
		}
	}

	// 8. Begin streaming on both OUTPUT and CAPTURE queues via VIDIOC_STREAMON().
	// This may be performed in any order.

	int buftype = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	xioctl(fd, VIDIOC_STREAMON, &buftype);

	buftype = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
	xioctl(fd, VIDIOC_STREAMON, &buftype);

	// Then enqueue all the capture buffers, to let the driver put encoded frames in them
	for (size_t i = 0; i < captureBufCount; ++i) {
		struct v4l2_buffer buffer;
		memset(&buffer, 0, sizeof(buffer));
		struct v4l2_plane plane;
		memset(&plane, 0, sizeof(plane));
		buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		buffer.index = i;
		buffer.length = 1;
		buffer.m.planes = &plane;
		buffer.memory = V4L2_MEMORY_MMAP;
		xioctl(fd, VIDIOC_QBUF, &buffer);
	}

	// This is the main loop, where we dequeue and re-enqueue available CAPTURE buffers,
	// dequeue available OUTPUT buffers, and write frames to the OUTPUT
	uint8_t fill = 0;
	while (1) {
		// Handle events from the driver
		struct pollfd pfd = {fd, POLLIN | POLLOUT, 0};
		while (1) {
			int ret = poll(&pfd, 1, 0);
			if (ret < 0 && errno == EINTR) {
				continue;
			} else if (ret < 0) {
				fprintf(stderr, "Poll error: %s\n", strerror(errno));
				return 1;
			} else if (ret == 0) {
				break;
			}

			if (pfd.revents & POLLIN) {
				// A capture buffer is ready, we have encoded data!
				struct v4l2_buffer buffer;
				memset(&buffer, 0, sizeof(buffer));
				struct v4l2_plane plane;
				memset(&plane, 0, sizeof(plane));
				buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
				buffer.memory = V4L2_MEMORY_MMAP;
				buffer.length = 1;
				buffer.m.planes = &plane;
				xioctl(fd, VIDIOC_DQBUF, &buffer);

				// Do something with the data
				struct mmbuffer *buf = &captureBufs[buffer.index];
				fprintf(stderr, "Capture buffer %i dequeued (at: %p, length: %i)\n", buffer.index, buf->start, plane.bytesused);

				// Re-enqueue the buffer
				size_t index = buffer.index;
				memset(&buffer, 0, sizeof(buffer));
				memset(&plane, 0, sizeof(plane));
				buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
				buffer.memory = V4L2_MEMORY_MMAP;
				buffer.length = 1;
				buffer.m.planes = &plane;
				buffer.index = index;
				xioctl(fd, VIDIOC_QBUF, &buffer);

				fprintf(stderr, "Capture buffer %i enqueued\n", buffer.index);
			}

			if (pfd.revents & POLLOUT) {
				// An output buffer is ready, dequeue it and mark it ready
				struct v4l2_buffer buffer;
				memset(&buffer, 0, sizeof(buffer));
				struct v4l2_plane plane;
				memset(&plane, 0, sizeof(plane));
				buffer.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
				buffer.memory = V4L2_MEMORY_MMAP;
				buffer.length = 1;
				buffer.m.planes = &plane;
				if (ioctl(fd, VIDIOC_DQBUF, &buffer) < 0) {
					fprintf(stderr, "VIDIOC_DQBUF (output): %s\n", strerror(errno));
					return 1;
				}

				fprintf(stderr, "Output buffer %i dequeued, marking ready\n", buffer.index);
				outputBufs[buffer.index].ready = 1;
			}

			if (pfd.revents & ~(POLLIN | POLLOUT)) {
				fprintf(stderr, "Unexpected revents: %i. Error?\n", pfd.revents);
				return 1;
			}
		}

		// Find an available output buffer
		int outputIdx = -1;
		struct mmbuffer *outputBuf = NULL;
		for (size_t i = 0; i < outputBufCount; ++i) {
			if (outputBufs[i].ready) {
				outputIdx = i;
				outputBuf = &outputBufs[i];
				break;
			}
		}

		// Produce a raw frame and queue it, if possible
		if (outputBuf) {
			size_t len = width * height + width * height / 2;
			memset(outputBuf->start, fill, len);
			fill += 1;

			struct v4l2_buffer buffer;
			memset(&buffer, 0, sizeof(buffer));
			struct v4l2_plane plane;
			memset(&plane, 0, sizeof(plane));
			buffer.type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
			buffer.length = 1;
			buffer.m.planes = &plane;
			buffer.memory = V4L2_MEMORY_MMAP;
			buffer.index = outputIdx;
			plane.bytesused = len;

			if (ioctl(fd, VIDIOC_QBUF, &buffer) < 0) {
				fprintf(stderr, "VIDIOC_QBUF (output): %s\n", strerror(errno));
				return 1;
			}

			fprintf(stderr, "Output buffer %i enqueued, marking not ready\n", buffer.index);
			outputBufs[outputIdx].ready = 0;
		} else {
			fprintf(stderr, "No output buffers ready!\n");
		}

		usleep(33 * 1000);
	}
}

// Example execution on my hardware:
/*
Found Qualcomm Venus video encoder (/dev/video5, fd 8)
Output format 0: NV12: Y/CbCr 4:2:0
Capture format 0: MPG4: MPEG-4 Part 2 ES
Capture format 1: H263: H.263
Capture format 2: H264: H.264
Capture format 3: VP80: VP8
Mapped buffer 0: 0xffffb40d0000, 1048576
Mapped buffer 1: 0xffffb3fd0000, 1048576
Mapped buffer 2: 0xffffb3ed0000, 1048576
Mapped buffer 3: 0xffffb3dd0000, 1048576
Mapped buffer 0: 0xffffb3d5c000, 475136
Mapped buffer 1: 0xffffb3ce8000, 475136
Mapped buffer 2: 0xffffb3c74000, 475136
Mapped buffer 3: 0xffffb3c00000, 475136
Mapped buffer 4: 0xffffb3b8c000, 475136
Output buffer 0 enqueued, marking not ready
Capture buffer 0 dequeued (at: 0xffffb40d0000, length: 1264)
Capture buffer 0 enqueued
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
Output buffer 0 dequeued, marking ready
Output buffer 0 enqueued, marking not ready
*/