v4l2_stateful_encoder.c

/*
 * Copyright 2021 The Chromium OS Authors. All rights reserved.
 * Use of this source code is governed by a BSD-style license that can be
 * found in the LICENSE file.
 */
// https://www.kernel.org/doc/html/latest/userspace-api/media/v4l/dev-encoder.html
#include <assert.h>
#include <ctype.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <linux/videodev2.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

static const char *kEncodeDevice = "/dev/video-enc0";
static const int kInputbufferMaxSize = 4 * 1024 * 1024;
static const int kRequestBufferCount = 8;
static const uint32_t kIVFHeaderSignature = v4l2_fourcc('D', 'K', 'I', 'F');

struct mmap_buffers {
	void *start[VIDEO_MAX_PLANES];
	size_t length[VIDEO_MAX_PLANES];
	struct gbm_bo *bo;
};

struct queue {
	int v4lfd;
	enum v4l2_buf_type type;
	uint32_t fourcc;
	struct mmap_buffers *buffers;
	uint32_t raw_width;
	uint32_t raw_height;
	uint32_t encoded_width;
	uint32_t encoded_height;
	uint32_t cnt;
	uint32_t frame_cnt;
	uint32_t num_planes;
	uint32_t framerate;
};

struct encoder_cfg {
	uint32_t gop_size;
	uint32_t bitrate;
	enum v4l2_mpeg_video_h264_entropy_mode h264_entropy_mode;
	enum v4l2_mpeg_video_h264_level h264_level;
	enum v4l2_mpeg_video_h264_profile h264_profile;
	enum v4l2_mpeg_video_header_mode header_mode;
	enum v4l2_mpeg_video_bitrate_mode bitrate_mode;
};

struct ivf_file_header {
	uint32_t signature;
	uint16_t version;
	uint16_t header_length;
	uint32_t fourcc;
	uint16_t width;
	uint16_t height;
	uint32_t denominator;
	uint32_t numerator;
	uint32_t frame_cnt;
	uint32_t unused;
} __attribute__((packed));

struct ivf_frame_header {
	uint32_t size;
	uint64_t timestamp;
} __attribute__((packed));

void print_fourcc(uint32_t fourcc)
{
	printf("%c%c%c%c\n", fourcc & 0xff, fourcc >> 8 & 0xff, fourcc >> 16 & 0xff,
	       fourcc >> 24 & 0xff);
}

int query_format(int v4lfd, enum v4l2_buf_type type, uint32_t fourcc)
{
	struct v4l2_fmtdesc fmtdesc;
	memset(&fmtdesc, 0, sizeof(fmtdesc));

	fmtdesc.type = type;
	while (ioctl(v4lfd, VIDIOC_ENUM_FMT, &fmtdesc) == 0) {
		if (fourcc == 0)
			print_fourcc(fmtdesc.pixelformat);
		else if (fourcc == fmtdesc.pixelformat)
			return 1;
		fmtdesc.index++;
	}

	return 0;
}

void enumerate_menu(int v4lfd, uint32_t id, uint32_t min, uint32_t max)
{
	struct v4l2_querymenu querymenu;
	memset(&querymenu, 0, sizeof(querymenu));

	querymenu.id = id;
	for (querymenu.index = min; querymenu.index <= max; querymenu.index++) {
		if (0 == ioctl(v4lfd, VIDIOC_QUERYMENU, &querymenu))
			fprintf(stderr, " %s\n", querymenu.name);
	}
}

int capabilities(int v4lfd, uint32_t OUTPUT_format, uint32_t CAPTURE_format,
		 int verbose_capabilities)
{
	struct v4l2_capability cap;
	memset(&cap, 0, sizeof(cap));
	int ret = ioctl(v4lfd, VIDIOC_QUERYCAP, &cap);
	if (ret != 0)
		perror("VIDIOC_QUERYCAP failed");

	printf("driver=\"%s\" bus_info=\"%s\" card=\"%s\" fd=0x%x\n", cap.driver, cap.bus_info,
	       cap.card, v4lfd);

	if (!query_format(v4lfd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, OUTPUT_format)) {
		printf("Supported OUTPUT formats:\n");
		query_format(v4lfd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 0);
		ret = 1;
	}

	if (!query_format(v4lfd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, CAPTURE_format)) {
		printf("Supported CAPTURE formats:\n");
		query_format(v4lfd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 0);
		ret = 1;
	}

	if (verbose_capabilities) {
		struct v4l2_query_ext_ctrl queryctrl;
		memset(&queryctrl, 0, sizeof(queryctrl));

		for (queryctrl.id = V4L2_CID_BASE; queryctrl.id < V4L2_CID_LASTP1; queryctrl.id++) {
			if (0 == ioctl(v4lfd, VIDIOC_QUERY_EXT_CTRL, &queryctrl)) {
				fprintf(stderr, "control %s : %s\n", queryctrl.name,
					queryctrl.flags & V4L2_CTRL_FLAG_DISABLED ? "disabled"
										  : "enabled");
				if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
					enumerate_menu(v4lfd, queryctrl.id, queryctrl.minimum,
						       queryctrl.maximum);
			} else if (errno == EINVAL) {
				continue;
			}
		}

		for (queryctrl.id = V4L2_CID_PRIVATE_BASE;; queryctrl.id++) {
			if (0 == ioctl(v4lfd, VIDIOC_QUERY_EXT_CTRL, &queryctrl)) {
				if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
					continue;

				fprintf(stderr, "control %s\n", queryctrl.name);

				if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
					enumerate_menu(v4lfd, queryctrl.id, queryctrl.minimum,
						       queryctrl.maximum);
			} else if (errno == EINVAL) {
				break;
			}
		}

		memset(&queryctrl, 0, sizeof(queryctrl));
		queryctrl.id = V4L2_CTRL_CLASS_MPEG | V4L2_CTRL_FLAG_NEXT_CTRL;
		while (0 == ioctl(v4lfd, VIDIOC_QUERY_EXT_CTRL, &queryctrl)) {
			fprintf(stderr, "control %s\n", queryctrl.name);

			if (queryctrl.type == V4L2_CTRL_TYPE_MENU)
				enumerate_menu(v4lfd, queryctrl.id, queryctrl.minimum,
					       queryctrl.maximum);

			if (V4L2_CTRL_ID2CLASS(queryctrl.id) != V4L2_CTRL_CLASS_MPEG)
				break;
			/* ... */
			queryctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
		}
	}
	return ret;
}

int queue_OUTPUT_buffer(struct queue *queue, struct mmap_buffers *buffers, uint32_t index)
{
	// compute frame timestamp
	const float usec_per_frame = (1.0 / queue->framerate) * 1000000;
	const uint64_t usec_time_stamp = usec_per_frame * queue->frame_cnt;
	const uint64_t tv_sec = usec_time_stamp / 1000000;

	struct v4l2_buffer v4l2_buffer;
	struct v4l2_plane planes[VIDEO_MAX_PLANES];
	memset(&v4l2_buffer, 0, sizeof(v4l2_buffer));

	v4l2_buffer.index = index;
	v4l2_buffer.type = queue->type;
	v4l2_buffer.memory = V4L2_MEMORY_MMAP;
	v4l2_buffer.length = queue->num_planes;
	v4l2_buffer.timestamp.tv_sec = tv_sec;
	v4l2_buffer.timestamp.tv_usec = usec_time_stamp - tv_sec;
	v4l2_buffer.sequence = queue->frame_cnt;
	v4l2_buffer.m.planes = planes;
	for (uint32_t i = 0; i < queue->num_planes; ++i) {
		v4l2_buffer.m.planes[i].length = buffers[index].length[i];
		v4l2_buffer.m.planes[i].bytesused = buffers[index].length[i];
		v4l2_buffer.m.planes[i].data_offset = 0;
	}

	int ret = ioctl(queue->v4lfd, VIDIOC_QBUF, &v4l2_buffer);
	if (ret != 0) {
		perror("VIDIOC_QBUF failed");
		return -1;
	}

	queue->frame_cnt++;

	return 0;
}

// This function copies the contents pointed by |fp| tp |queue|s |index| buffer.
int submit_raw_frame_in_bulk(FILE *fp, struct queue *queue, uint32_t index)
{
	assert(queue->num_planes == 1 || queue->num_planes == 2);
	assert(queue->raw_width == queue->encoded_width);
	// TODO: the code below assumes NV12 because the Chroma planes are copied in
	// one call. Extend to YV12 if ever the need arises.
	assert(queue->fourcc == v4l2_fourcc('N', 'V', '1', '2'));

	struct mmap_buffers *buffers = queue->buffers;

	// read y plane first
	size_t frame_size = queue->raw_width * queue->raw_height;
	uint8_t *buffer = buffers[index].start[0];

	if (fread(buffer, frame_size, 1, fp) != 1) {
		fprintf(stderr, "unable to read luma frame\n");
		return -1;
	}

	// now read uv
	frame_size >>= 1;
	if (queue->num_planes == 2)
		buffer = buffers[index].start[1];
	else
		buffer += queue->encoded_width * queue->encoded_height;

	if (fread(buffer, frame_size, 1, fp) != 1) {
		fprintf(stderr, "unable to read chroma frame\n");
		return -1;
	}

	return queue_OUTPUT_buffer(queue, buffers, index);
}

// This function copies the contents pointed by |fp| to |queue|s |index| buffer.
// It's assumed that
int submit_raw_frame_row_by_row(FILE *fp, uint32_t file_format, struct queue *queue, uint32_t index)
{
	assert(queue->num_planes == 1 || queue->num_planes == 2);
	assert(queue->fourcc == v4l2_fourcc('Y', 'V', '1', '2') ||
	       queue->fourcc == v4l2_fourcc('N', 'V', '1', '2'));
	assert(file_format == v4l2_fourcc('Y', 'V', '1', '2') ||
	       file_format == v4l2_fourcc('N', 'V', '1', '2'));

	struct mmap_buffers *buffers = queue->buffers;

	// Read Y plane first, row by row.
	uint8_t *buffer = buffers[index].start[0];
	for (int row = 0; row < queue->raw_height; ++row) {
		if (fread(buffer, queue->raw_width, 1, fp) != 1) {
			fprintf(stderr, "unable to read luma row\n");
			return -1;
		}
		buffer += queue->encoded_width;
	}

	if (queue->num_planes == 2)
		buffer = buffers[index].start[1];
	else
		buffer = buffers[index].start[0] + queue->encoded_width * queue->encoded_height;

	// Now read the U and V planes.
	if (queue->fourcc == v4l2_fourcc('Y', 'V', '1', '2') &&
	    file_format == v4l2_fourcc('Y', 'V', '1', '2')) {
		printf("copying YV12 to YV12\n");
		for (int row = 0; row < queue->raw_height / 4; ++row) {
			if (fread(buffer, queue->raw_width, 1, fp) != 1) {
				fprintf(stderr, "unable to read chroma row\n");
				return -1;
			}
			buffer += queue->encoded_width;
		}

		if (queue->num_planes == 2) {
			buffer = buffers[index].start[1] +
				 queue->encoded_width * queue->encoded_height / 4;
		} else {
			buffer = buffers[index].start[0] +
				 5 * queue->encoded_width * queue->encoded_height / 4;
		}

		for (int row = 0; row < queue->raw_height / 4; ++row) {
			if (fread(buffer, queue->raw_width, 1, fp) != 1) {
				fprintf(stderr, "unable to read chroma row\n");
				return -1;
			}
			buffer += queue->encoded_width;
		}
	} else if (queue->fourcc == v4l2_fourcc('N', 'V', '1', '2') &&
		   file_format == v4l2_fourcc('Y', 'V', '1', '2') && queue->num_planes == 1) {
		const int kNumPlanes = 2u;
		for (int plane = 0; plane < kNumPlanes; ++plane) {
			// Copy all chroma samples from |fp| one by one in even |buffer| positions,
			// then rewind |buffer|, move it one position right and copy from |fp| into
			// the odd |buffer| positions.
			for (int row = 0; row < queue->raw_height / 4; ++row) {
				for (int col = 0; col < queue->raw_width / 2; ++col) {
					if (fread(buffer, 1 /*size */, 1 /*nmemb*/, fp) != 1) {
						fprintf(stderr, "unable to read chroma byte\n");
						return -1;
					}
					buffer += 2;
				}
				buffer += queue->encoded_width - queue->raw_width;

				for (int col = 0; col < queue->raw_width / 2; ++col) {
					if (fread(buffer, 1 /*size */, 1 /*nmemb*/, fp) != 1) {
						fprintf(stderr, "unable to read chroma byte\n");
						return -1;
					}
					buffer += 2;
				}
				buffer += queue->encoded_width - queue->raw_width;
			}
			// Rewind |buffer| to start writing the other Chroma samples.
			buffer -= queue->encoded_width * queue->raw_height / 2;
			buffer++;		}

	} else {
		fprintf(
		    stderr,
		    "combination of queue format, number of planes, and file format unsupported\n");
		return -1;
	}

	return queue_OUTPUT_buffer(queue, buffers, index);
}

// This function copies the content of |fp| into the |index|th buffer of
// |queue|. Depending on |file_format| and the |queue| format, and the raw and
// encoded sizes of the latter, we might do a copy in bulk or need conversion.
int submit_raw_frame(FILE *fp, uint32_t file_format, struct queue *queue, uint32_t index)
{
	if (queue->raw_width == queue->encoded_width && queue->fourcc == file_format &&
	    queue->fourcc == v4l2_fourcc('N', 'V', '1', '2')) {
		return submit_raw_frame_in_bulk(fp, queue, index);
	}

	return submit_raw_frame_row_by_row(fp, file_format, queue, index);
}

void cleanup_queue(struct queue *queue)
{
	if (queue->cnt) {
		struct mmap_buffers *buffers = queue->buffers;

		for (uint32_t i = 0; i < queue->cnt; i++)
			for (uint32_t j = 0; j < queue->num_planes; j++) {
				munmap(buffers[i].start[j], buffers[i].length[j]);
			}

		free(queue->buffers);
		queue->cnt = 0;
	}
}

int request_mmap_buffers(struct queue *queue, struct v4l2_requestbuffers *reqbuf)
{
	const int v4lfd = queue->v4lfd;
	const uint32_t buffer_alloc = reqbuf->count * sizeof(struct mmap_buffers);
	struct mmap_buffers *buffers = (struct mmap_buffers *)malloc(buffer_alloc);
	memset(buffers, 0, buffer_alloc);
	queue->buffers = buffers;
	queue->cnt = reqbuf->count;

	int ret;
	for (uint32_t i = 0; i < reqbuf->count; i++) {
		struct v4l2_buffer buffer;
		struct v4l2_plane planes[VIDEO_MAX_PLANES];
		memset(&buffer, 0, sizeof(buffer));
		buffer.type = reqbuf->type;
		buffer.memory = V4L2_MEMORY_MMAP;
		buffer.index = i;
		buffer.length = queue->num_planes;
		buffer.m.planes = planes;
		ret = ioctl(v4lfd, VIDIOC_QUERYBUF, &buffer);
		if (ret != 0) {
			printf("VIDIOC_QUERYBUF failed: %d\n", ret);
			break;
		}

		for (uint32_t j = 0; j < queue->num_planes; j++) {
			buffers[i].length[j] = buffer.m.planes[j].length;
			buffers[i].start[j] =
			    mmap(NULL, buffer.m.planes[j].length, PROT_READ | PROT_WRITE,
				 MAP_SHARED, v4lfd, buffer.m.planes[j].m.mem_offset);
			if (MAP_FAILED == buffers[i].start[j]) {
				fprintf(stderr,
					"failed to mmap buffer of length(%d) and offset(0x%x)\n",
					buffer.m.planes[j].length, buffer.m.planes[j].m.mem_offset);
			}
		}
	}

	return ret;
}

int queue_CAPTURE_buffer(struct queue *queue, uint32_t index)
{
	struct mmap_buffers *buffers = queue->buffers;
	struct v4l2_buffer v4l2_buffer;
	struct v4l2_plane planes[VIDEO_MAX_PLANES];
	memset(&v4l2_buffer, 0, sizeof v4l2_buffer);
	memset(&planes, 0, sizeof planes);

	v4l2_buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
	v4l2_buffer.memory = V4L2_MEMORY_MMAP;
	v4l2_buffer.index = index;
	v4l2_buffer.m.planes = planes;
	v4l2_buffer.length = queue->num_planes;

	v4l2_buffer.m.planes[0].length = buffers[index].length[0];
	v4l2_buffer.m.planes[0].bytesused = buffers[index].length[0];
	v4l2_buffer.m.planes[0].data_offset = 0;

	int ret = ioctl(queue->v4lfd, VIDIOC_QBUF, &v4l2_buffer);
	if (ret != 0) {
		perror("VIDIOC_QBUF failed");
	}

	return ret;
}

// 4.5.2.5. Initialization
int Initialization(struct queue *OUTPUT_queue, struct queue *CAPTURE_queue)
{
	int ret = 0;

	// 1. Set the coded format on the CAPTURE queue via VIDIOC_S_FMT().
	if (!ret) {
		struct v4l2_format fmt;
		memset(&fmt, 0, sizeof(fmt));

		fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
		fmt.fmt.pix_mp.pixelformat = CAPTURE_queue->fourcc;
		fmt.fmt.pix_mp.width = CAPTURE_queue->raw_width;
		fmt.fmt.pix_mp.height = CAPTURE_queue->raw_height;
		fmt.fmt.pix_mp.plane_fmt[0].sizeimage = kInputbufferMaxSize;
		fmt.fmt.pix_mp.num_planes = 1;

		int ret = ioctl(CAPTURE_queue->v4lfd, VIDIOC_S_FMT, &fmt);
		if (ret != 0)
			perror("VIDIOC_S_FMT failed");

		CAPTURE_queue->encoded_width = fmt.fmt.pix_mp.width;
		CAPTURE_queue->encoded_height = fmt.fmt.pix_mp.height;
	}

	// 3. Set the raw source format on the OUTPUT queue via VIDIOC_S_FMT().
	if (!ret) {
		struct v4l2_format fmt;
		memset(&fmt, 0, sizeof(fmt));

		fmt.type = OUTPUT_queue->type;
		fmt.fmt.pix_mp.pixelformat = OUTPUT_queue->fourcc;
		fmt.fmt.pix_mp.width = OUTPUT_queue->raw_width;
		fmt.fmt.pix_mp.height = OUTPUT_queue->raw_height;
		fmt.fmt.pix_mp.num_planes = 1;

		int ret = ioctl(OUTPUT_queue->v4lfd, VIDIOC_S_FMT, &fmt);
		if (ret != 0)
			perror("VIDIOC_S_FMT failed");

		OUTPUT_queue->encoded_width = fmt.fmt.pix_mp.width;
		OUTPUT_queue->encoded_height = fmt.fmt.pix_mp.height;

		OUTPUT_queue->num_planes = fmt.fmt.pix_mp.num_planes;
	}

	// 4. Set the raw frame interval on the OUTPUT queue via VIDIOC_S_PARM()
	if (!ret) {
		struct v4l2_streamparm parms;
		memset(&parms, 0, sizeof(parms));
		parms.type = OUTPUT_queue->type;
		// Note that we are provided "frames per second" but V4L2 expects "time per
		// frame"; hence we provide the reciprocal of the framerate here.
		parms.parm.output.timeperframe.numerator = 1;
		parms.parm.output.timeperframe.denominator = OUTPUT_queue->framerate;

		ret = ioctl(OUTPUT_queue->v4lfd, VIDIOC_S_PARM, &parms);
		if (ret != 0)
			perror("VIDIOC_S_PARAM failed");
	}

	// 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.
	if (!ret) {
		struct v4l2_selection selection_arg;
		memset(&selection_arg, 0, sizeof(selection_arg));
		selection_arg.type = OUTPUT_queue->type;
		selection_arg.target = V4L2_SEL_TGT_CROP;
		selection_arg.r.left = 0;
		selection_arg.r.top = 0;
		selection_arg.r.width = OUTPUT_queue->raw_width;
		selection_arg.r.height = OUTPUT_queue->raw_height;

		ret = ioctl(OUTPUT_queue->v4lfd, VIDIOC_S_SELECTION, &selection_arg);

		if (ret != 0)
			perror("VIDIOC_S_SELECTION failed");

		// TODO(fritz) : check returned values are same as sent values
	}

	// 7. Allocate buffers for both OUTPUT and CAPTURE via VIDIOC_REQBUFS().
	//    This may be performed in any order.
	if (!ret) {
		struct v4l2_requestbuffers reqbuf;
		memset(&reqbuf, 0, sizeof(reqbuf));
		reqbuf.count = kRequestBufferCount;
		reqbuf.type = OUTPUT_queue->type;
		reqbuf.memory = V4L2_MEMORY_MMAP;

		ret = ioctl(OUTPUT_queue->v4lfd, VIDIOC_REQBUFS, &reqbuf);
		if (ret != 0)
			perror("VIDIOC_REQBUFS failed");

		printf("%d buffers requested, %d buffers for uncompressed frames returned\n",
		       kRequestBufferCount, reqbuf.count);

		ret = request_mmap_buffers(OUTPUT_queue, &reqbuf);
	}

	if (!ret) {
		struct v4l2_requestbuffers reqbuf;
		memset(&reqbuf, 0, sizeof(reqbuf));
		reqbuf.count = kRequestBufferCount;
		reqbuf.type = CAPTURE_queue->type;
		reqbuf.memory = V4L2_MEMORY_MMAP;

		ret = ioctl(OUTPUT_queue->v4lfd, VIDIOC_REQBUFS, &reqbuf);
		if (ret != 0)
			perror("VIDIOC_REQBUFS failed");

		printf("%d buffers requested, %d buffers for compressed frames returned\n",
		       kRequestBufferCount, reqbuf.count);

		ret = request_mmap_buffers(CAPTURE_queue, &reqbuf);
		for (uint32_t i = 0; i < reqbuf.count; i++) {
			queue_CAPTURE_buffer(CAPTURE_queue, i);
		}
	}

	return ret;
}

int configure_h264(int v4lfd, struct encoder_cfg *cfg)
{
	int ret = 0;
	const int kH264CtrlCnt = 4;

	struct v4l2_ext_control ext_ctrl[kH264CtrlCnt];
	memset(&ext_ctrl, 0, sizeof(ext_ctrl));

	ext_ctrl[0].id = V4L2_CID_MPEG_VIDEO_H264_PROFILE;
	ext_ctrl[0].value = cfg->h264_profile;

	ext_ctrl[1].id = V4L2_CID_MPEG_VIDEO_H264_LEVEL;
	ext_ctrl[1].value = cfg->h264_level;

	ext_ctrl[2].id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE;
	ext_ctrl[2].value = cfg->h264_entropy_mode;

	ext_ctrl[3].id = V4L2_CID_MPEG_VIDEO_HEADER_MODE;
	ext_ctrl[3].value = cfg->header_mode;

	struct v4l2_ext_controls ext_ctrls;
	memset(&ext_ctrls, 0, sizeof(ext_ctrls));

	ext_ctrls.ctrl_class = V4L2_CTRL_CLASS_MPEG;
	ext_ctrls.count = kH264CtrlCnt;
	ext_ctrls.controls = ext_ctrl;

	ret = ioctl(v4lfd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);

	if (ret != 0)
		perror("VIDIOC_S_EXT_CTRLS failed");

	for (uint32_t i = 0; i < kH264CtrlCnt; ++i)
		ext_ctrl[i].value = 0;

	ret = ioctl(v4lfd, VIDIOC_G_EXT_CTRLS, &ext_ctrls);
	if (ret != 0)
		perror("VIDIOC_G_EXT_CTRLS failed");

	if (ext_ctrl[0].value != cfg->h264_profile)
		fprintf(stderr, "requested profile(%d) was not used, using (%d) instead.\n",
			cfg->h264_profile, ext_ctrl[0].value);

	if (ext_ctrl[1].value != cfg->h264_level)
		fprintf(stderr, "requested level(%d) was not used, using (%d) instead.\n",
			cfg->h264_level, ext_ctrl[1].value);

	if (ext_ctrl[2].value != cfg->h264_entropy_mode)
		fprintf(stderr, "requested entropy mode(%d) was not used, using (%d) instead.\n",
			cfg->h264_entropy_mode, ext_ctrl[2].value);

	if (ext_ctrl[3].value != cfg->header_mode)
		fprintf(stderr, "requested entropy mode(%d) was not used, using (%d) instead.\n",
			cfg->header_mode, ext_ctrl[3].value);

	return ret;
}

int configure_common(int v4lfd, struct encoder_cfg *cfg)
{
	int ret = 0;
	const int kCommonCtrlCnt = 5;

	struct v4l2_ext_control ext_ctrl[kCommonCtrlCnt];
	memset(&ext_ctrl, 0, sizeof(ext_ctrl));

	ext_ctrl[0].id = V4L2_CID_MPEG_VIDEO_BITRATE;
	ext_ctrl[0].value = cfg->bitrate;

	ext_ctrl[1].id = V4L2_CID_MPEG_VIDEO_BITRATE_PEAK;
	ext_ctrl[1].value = cfg->bitrate * 2;

	ext_ctrl[2].id = V4L2_CID_MPEG_VIDEO_GOP_SIZE;
	ext_ctrl[2].value = cfg->gop_size;

	ext_ctrl[3].id = V4L2_CID_MPEG_VIDEO_FRAME_RC_ENABLE;
	ext_ctrl[3].value = 1;

	ext_ctrl[4].id = V4L2_CID_MPEG_VIDEO_BITRATE_MODE;
	ext_ctrl[4].value = cfg->bitrate_mode;

	struct v4l2_ext_controls ext_ctrls;
	memset(&ext_ctrls, 0, sizeof(ext_ctrls));

	ext_ctrls.ctrl_class = V4L2_CTRL_CLASS_MPEG;
	ext_ctrls.count = kCommonCtrlCnt;
	ext_ctrls.controls = ext_ctrl;

	ret = ioctl(v4lfd, VIDIOC_S_EXT_CTRLS, &ext_ctrls);

	if (ret != 0)
		perror("VIDIOC_S_EXT_CTRLS failed");

	for (uint32_t i = 0; i < kCommonCtrlCnt; ++i)
		ext_ctrl[i].value = 0;

	ret = ioctl(v4lfd, VIDIOC_G_EXT_CTRLS, &ext_ctrls);
	if (ret != 0)
		perror("VIDIOC_G_EXT_CTRLS failed");

	if (ext_ctrl[0].value != cfg->bitrate)
		fprintf(stderr,
			"requested bitrate(%d) was outside of the limit, using (%d) instead.\n",
			cfg->bitrate, ext_ctrl[0].value);

	if (ext_ctrl[1].value != cfg->bitrate * 2)
		fprintf(
		    stderr,
		    "requested bitrate peak(%d) was outside of the limit, using (%d) instead.\n",
		    cfg->bitrate * 2, ext_ctrl[1].value);

	if (ext_ctrl[2].value != cfg->gop_size)
		fprintf(stderr, "requested gop size(%d) was not used, using (%d) instead.\n",
			cfg->gop_size, ext_ctrl[2].value);

	if (ext_ctrl[3].value != 1)
		fprintf(stderr,
			"requested frame rate control (%d) was not used, using (%d) instead.\n", 1,
			ext_ctrl[3].value);

	if (ext_ctrl[4].value != cfg->bitrate_mode)
		fprintf(stderr, "requested bitrate mode(%d) was not used, using (%d) instead.\n",
			cfg->bitrate_mode, ext_ctrl[4].value);

	return ret;
}

int dequeue_buffer(struct queue *queue, uint32_t *index, uint32_t *bytesused, uint32_t *data_offset,
		   uint64_t *timestamp)
{
	struct v4l2_buffer v4l2_buffer;
	struct v4l2_plane planes[VIDEO_MAX_PLANES] = { 0 };
	memset(&v4l2_buffer, 0, sizeof(v4l2_buffer));
	v4l2_buffer.type = queue->type;
	v4l2_buffer.length = queue->num_planes;
	v4l2_buffer.m.planes = planes;
	v4l2_buffer.m.planes[0].bytesused = 0;
	int ret = ioctl(queue->v4lfd, VIDIOC_DQBUF, &v4l2_buffer);

	if (ret != 0 && errno != EAGAIN)
		perror("VIDIOC_DQBUF failed");

	*index = v4l2_buffer.index;
	if (bytesused)
		*bytesused = v4l2_buffer.m.planes[0].bytesused;
	if (data_offset)
		*data_offset = v4l2_buffer.m.planes[0].data_offset;
	if (timestamp)
		*timestamp = v4l2_buffer.timestamp.tv_usec;
	return ret;
}

int encode(FILE *fp_input, uint32_t file_format, char* output_file_name, struct queue *OUTPUT_queue, struct queue *CAPTURE_queue,
	   uint32_t frames_to_decode)
{
	if (OUTPUT_queue->num_planes == 0 || OUTPUT_queue->num_planes > 2) {
		fprintf(stderr, " unsupported number of planes: %d\n", OUTPUT_queue->num_planes);
		return -1;
	}
	fprintf(stderr, "encoding\n");

	const int use_ivf = CAPTURE_queue->fourcc == v4l2_fourcc('V', 'P', '8', '0');
	strcat(output_file_name, use_ivf ? ".ivf" : ".h264");

	int ret = 0;
	FILE *fp_output = fopen(output_file_name, "wb");
	if (!fp_output) {
		fprintf(stderr, "unable to write to file: %s\n", output_file_name);
		ret = 1;
	}

	// write header
	if (use_ivf) {
		struct ivf_file_header header;
		header.signature = kIVFHeaderSignature;
		header.version = 0;
		header.header_length = sizeof(struct ivf_file_header);
		header.fourcc = CAPTURE_queue->fourcc;
		header.width = CAPTURE_queue->raw_width;
		header.height = CAPTURE_queue->raw_height;
		// hard coded 30fps
		header.denominator = 30;
		header.numerator = 1;
		header.frame_cnt = frames_to_decode;
		header.unused = 0;

		if (fwrite(&header, sizeof(struct ivf_file_header), 1, fp_output) != 1) {
			fprintf(stderr, "unable to write ivf file header\n");
		}
	}

	struct timespec start, stop;
	clock_gettime(CLOCK_REALTIME, &start);
	if (!ret) {
		// prime input by filling up the OUTPUT queue with raw frames
		for (uint32_t i = 0; i < OUTPUT_queue->cnt; ++i) {
			if (submit_raw_frame(fp_input, file_format, OUTPUT_queue, i)) {
				fprintf(stderr, "unable to submit raw frame\n");
				ret = 1;
			}
		}
	}

	if (!ret) {
		ret = ioctl(OUTPUT_queue->v4lfd, VIDIOC_STREAMON, &OUTPUT_queue->type);
		if (ret != 0)
			perror("VIDIOC_STREAMON failed on OUTPUT");
	}

	if (!ret) {
		ret = ioctl(CAPTURE_queue->v4lfd, VIDIOC_STREAMON, &CAPTURE_queue->type);
		if (ret != 0)
			perror("VIDIOC_STREAMON failed on CAPTURE");
	}

	uint32_t cnt = OUTPUT_queue->cnt;  // We pre-uploaded a few before.
	if (!ret) {
		while (cnt < frames_to_decode) {
			// handle CAPTURE queue first
			{
				uint32_t index = 0;
				uint32_t bytesused = 0;
				uint32_t data_offset = 0;
				uint64_t timestamp = 0;

				// first get the newly encoded frame
				ret = dequeue_buffer(CAPTURE_queue, &index, &bytesused,
						     &data_offset, &timestamp);
				if (ret != 0)
					continue;

				if (use_ivf) {
					struct ivf_frame_header header;
					header.size = bytesused - data_offset;
					header.timestamp = timestamp;

					if (fwrite(&header, sizeof(struct ivf_frame_header), 1,
						   fp_output) != 1) {
						fprintf(stderr,
							"unable to write ivf frame header\n");
					}
				}
				fwrite(CAPTURE_queue->buffers[index].start[0] + data_offset,
				       bytesused - data_offset, 1, fp_output);

				// done with the buffer, queue it back up
				queue_CAPTURE_buffer(CAPTURE_queue, index);
			}

			// handle OUTPUT queue second
			{
				uint32_t index = 0;

				ret = dequeue_buffer(OUTPUT_queue, &index, 0, 0, 0);
				if (ret != 0)
					continue;

				if (submit_raw_frame(fp_input, file_format, OUTPUT_queue, index))
					break;
			}
			cnt++;
		}
	}
	clock_gettime(CLOCK_REALTIME, &stop);
	const double elapsed_ns =
	    (stop.tv_sec - start.tv_sec) * 1e9 + (stop.tv_nsec - start.tv_nsec);
	const double fps = cnt * 1e9 / elapsed_ns;
	printf("%d frames encoded in %fns (%ffps)\n", cnt, elapsed_ns, fps);

	if (fp_output) {
		fclose(fp_output);
	}
	return ret;
}

static void print_help(const char *argv0)
{
	printf("usage: %s [OPTIONS]\n", argv0);
	printf("  -f, --file        file to encode\n");
	printf("  -i, --file_format pixel format of the file (yv12, nv12)\n");
	printf("  -o, --output      output file name (will get a .h264 or .ivf suffix added)\n");
	printf("  -w, --width       width of image\n");
	printf("  -h, --height      height of image\n");
	printf("  -m, --max         max number of frames to decode\n");
	printf("  -r, --rate        frames per second\n");
	printf("  -b, --bitrate     bits per second\n");
	printf("  -g, --gop         gop length\n");
	printf("  -c, --codec       codec\n");
	printf("  -e, --end_usage   rate control mode: VBR (default), CBR\n");
	printf("  -v, --verbose     verbose capabilities\n");
	printf("  -q, --buffer_fmt  OUTPUT queue format\n");
}

static const struct option longopts[] = {
	{ "file", required_argument, NULL, 'f' },
	{ "file_format", required_argument, NULL, 'i' },
	{ "output", required_argument, NULL, 'o' },
	{ "width", required_argument, NULL, 'w' },
	{ "height", required_argument, NULL, 'h' },
	{ "max", required_argument, NULL, 'm' },
	{ "fps", required_argument, NULL, 'r' },
	{ "bitrate", required_argument, NULL, 'b' },
	{ "gop", required_argument, NULL, 'g' },
	{ "codec", required_argument, NULL, 'c' },
	{ "end_usage", required_argument, NULL, 'e' },
	{ "verbose", no_argument, NULL, 'v' },
	{ "buffer_fmt", required_argument, NULL, 'q' },
	{ 0, 0, 0, 0 },
};

int main(int argc, char *argv[])
{
	uint32_t file_format = v4l2_fourcc('N', 'V', '1', '2');
	uint32_t OUTPUT_format = v4l2_fourcc('N', 'V', '1', '2');
	uint32_t CAPTURE_format = v4l2_fourcc('H', '2', '6', '4');
	char *file_name = NULL;
	char *output_file_name = NULL;
	uint32_t width = 0;
	uint32_t height = 0;
	uint32_t frames_to_decode = 0;
	uint32_t framerate = 30;
	int verbose_capabilities = 0;
	int c;

	struct encoder_cfg cfg = { .gop_size = 20,
				   .bitrate = 1000,
				   .h264_entropy_mode = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC,
				   .h264_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0,
				   .h264_profile = V4L2_MPEG_VIDEO_H264_PROFILE_MAIN,
				   .header_mode = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE,
				   .bitrate_mode = V4L2_MPEG_VIDEO_BITRATE_MODE_VBR };

	while ((c = getopt_long(argc, argv, "f:i:o:w:h:m:r:b:g:c:e:vq:", longopts, NULL)) != -1) {
		switch (c) {
			case 'f':
				file_name = strdup(optarg);
				break;
			case 'i':
				if (strlen(optarg) == 4) {
					file_format =
					    v4l2_fourcc(toupper(optarg[0]), toupper(optarg[1]),
							toupper(optarg[2]), toupper(optarg[3]));
					printf("using (%s) as the file format\n", optarg);
				}
				break;
			case 'o':
				output_file_name = strdup(optarg);
				break;
			case 'w':
				width = atoi(optarg);
				break;
			case 'h':
				height = atoi(optarg);
				break;
			case 'm':
				frames_to_decode = atoi(optarg);
				break;
			case 'r':
				framerate = atoi(optarg);
				break;
			case 'b':
				cfg.bitrate = atoi(optarg);
				break;
			case 'g':
				cfg.gop_size = atoi(optarg);
				break;
			case 'c':
				if (strlen(optarg) == 4) {
					CAPTURE_format =
					    v4l2_fourcc(toupper(optarg[0]), toupper(optarg[1]),
							toupper(optarg[2]), toupper(optarg[3]));
					printf("using (%s) as the codec\n", optarg);
				}
				break;
			case 'e':
				if (strlen(optarg) == 3 && toupper(optarg[0]) == 'C' &&
				    toupper(optarg[1]) == 'B' && toupper(optarg[2]) == 'R') {
					cfg.bitrate_mode = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR;
				}
				break;
			case 'v':
				verbose_capabilities = 1;
				break;
			case 'q':
				if (strlen(optarg) == 4) {
					OUTPUT_format =
					    v4l2_fourcc(toupper(optarg[0]), toupper(optarg[1]),
							toupper(optarg[2]), toupper(optarg[3]));
					printf("using (%s) as the OUTPUT queue buffer format\n",
					       optarg);
				}
				break;
			default:
				break;
		}
	}

	if (!file_name || !output_file_name || width == 0 || height == 0) {
		fprintf(stderr, "Invalid parameters!\n");
		print_help(argv[0]);
		exit(1);
	}

	FILE *fp = fopen(file_name, "rb");
	if (!fp) {
		fprintf(stderr, "%s: unable to open file.\n", file_name);
		exit(1);
	}

	if (!frames_to_decode) {
		fseek(fp, 0, SEEK_END);
		uint64_t length = ftell(fp);
		uint32_t frame_size = (3 * width * height) >> 1;
		frames_to_decode = length / frame_size;
		fseek(fp, 0, SEEK_SET);
	}

	fprintf(stderr, "encoding %d frames using %s bitrate control\n",
		frames_to_decode,
		(cfg.bitrate_mode = V4L2_MPEG_VIDEO_BITRATE_MODE_CBR) ? "CBR" : "VBR");

	int v4lfd = open(kEncodeDevice, O_RDWR | O_NONBLOCK | O_CLOEXEC);
	if (v4lfd < 0) {
		fprintf(stderr, "Unable to open device file: %s\n", kEncodeDevice);
		exit(EXIT_FAILURE);
	}

	if (capabilities(v4lfd, OUTPUT_format, CAPTURE_format, verbose_capabilities) != 0) {
		fprintf(stderr, "Capabilities not present for encode.\n");
		exit(EXIT_FAILURE);
	}

	struct queue OUTPUT_queue = { .v4lfd = v4lfd,
				      .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
				      .fourcc = OUTPUT_format,
				      .raw_width = width,
				      .raw_height = height,
				      .frame_cnt = 0,
				      .num_planes = 1,
				      .framerate = framerate };

	struct queue CAPTURE_queue = { .v4lfd = v4lfd,
				       .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
				       .fourcc = CAPTURE_format,
				       .raw_width = width,
				       .raw_height = height,
				       .num_planes = 1,
				       .framerate = framerate };

	int ret = Initialization(&OUTPUT_queue, &CAPTURE_queue);

	// not all configurations are supported, so we don't need to track
	// the return value
	if (!ret) {
		configure_common(v4lfd, &cfg);

		if (v4l2_fourcc('H', '2', '6', '4') == CAPTURE_format)
			configure_h264(v4lfd, &cfg);
	}

	if (!ret)
		ret = encode(fp, file_format, output_file_name, &OUTPUT_queue, &CAPTURE_queue, frames_to_decode);

	cleanup_queue(&OUTPUT_queue);
	cleanup_queue(&CAPTURE_queue);
	close(v4lfd);

	return 0;
}