stressjpeg.c

#include <linux/videodev2.h>
#include <linux/v4l2-subdev.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>

#include <unistd.h> // close
#include <sys/ioctl.h> // ioctl
#include <sys/mman.h> // mmap
#include <sys/epoll.h>

#include <stdint.h>
#include <stdio.h> // perror, fprintf
#include <stdlib.h> // abort

#include <assert.h>
#include <errno.h>
#include <string.h> // strerror

typedef uint32_t u32;

#define LOGF(msg, ...) fprintf(stderr, msg "\n", __VA_ARGS__)
#define LOG(msg) fprintf(stderr, msg "\n")

typedef struct {
	int fd;
	u32 buf_type;
	uint32_t pixelformat;
	int w, h;
	u32 buffers_count;
	u32 memory;
} BufferStart;

typedef struct {
	struct v4l2_buffer buffer;
	struct v4l2_plane planes[VIDEO_MAX_PLANES];
	void *mapped[VIDEO_MAX_PLANES];
	int enqueued;
} Buffer;

typedef struct {
	u32 type;
	struct v4l2_format fmt;
	Buffer *buffers;
	int buffers_count;
} Stream;

static Stream streamStart(BufferStart args) {
	const int planes_num = 1;
	struct v4l2_format fmt = {
		.type = args.buf_type,
		.fmt.pix_mp = {
			.width = args.w,
			.height = args.h,
			.pixelformat = args.pixelformat,
			.field = V4L2_FIELD_NONE,
			.colorspace = V4L2_COLORSPACE_REC709,
			.plane_fmt[0].sizeimage = 0,
			.plane_fmt[0].bytesperline = 0,
			.num_planes = planes_num,
		},
	};

	if (0 != ioctl(args.fd, VIDIOC_S_FMT, &fmt)) {
		perror("ioctl(VIDIOC_S_FMT)");
		abort();
	}

	LOGF("sizeimage=%u, .bytesperline=%u",
		fmt.fmt.pix_mp.plane_fmt[0].sizeimage, 
		fmt.fmt.pix_mp.plane_fmt[0].bytesperline);

	struct v4l2_requestbuffers req = {
		.type = args.buf_type,
		.count = args.buffers_count,
		.memory = args.memory,
	};
	if (0 > ioctl(args.fd, VIDIOC_REQBUFS, &req)) {
		perror("VIDIOC_REQBUFS");
		abort();
	}

	Buffer *const buffers = calloc(req.count, sizeof(*buffers));
	for (int i = 0; i < req.count; ++i) {
		Buffer *const buf = buffers + i;
		buf->buffer = (struct v4l2_buffer){
			.type = req.type,
			.memory = req.memory,
			.index = i,
			.m.planes = buf->planes,
			.length = planes_num,
		};

		if (0 != ioctl(args.fd, VIDIOC_QUERYBUF, &buf->buffer)) {
			LOGF("Failed to ioctl(%d, VIDIOC_QUERYBUF, [%d]): %d, %s", args.fd, i, errno, strerror(errno));
			abort();
		}

		for (int i = 0; i < planes_num; ++i) {
			const uint32_t offset = buf->buffer.m.planes[i].m.mem_offset;
			const uint32_t length = buf->buffer.m.planes[i].length;
			buf->mapped[i] = mmap(NULL, length, PROT_READ | PROT_WRITE, MAP_SHARED, args.fd, offset);
			if (buf->mapped[i] == MAP_FAILED) {
				LOGF("Failed to mmap(%d, buffer[%d]): %d, %s", args.fd, buf->buffer.index, errno, strerror(errno));
				abort();
			}

			LOGF("buf.index=%d plane=%d mmap=%p size=%u", buf->buffer.index, i, buf->mapped[i], length);
		}
	}

	if (args.buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
		for (int i = 0; i < req.count; ++i) {
			LOGF("%d QBUF", i);
			if (0 != ioctl(args.fd, VIDIOC_QBUF, &buffers[i].buffer)) {
				LOGF("Failed to ioctl(%d, VIDIOC_QBUF, %i): %d, %s",
					args.fd, i, errno, strerror(errno));
				abort();
			}
			buffers[i].enqueued = 1;
		}
	}

	if (0 != ioctl(args.fd, VIDIOC_STREAMON, &args.buf_type)) {
		LOGF("Failed to ioctl(%d, VIDIOC_STREAMON): %d, %s",
			args.fd, errno, strerror(errno));
		abort();
	}

	return (Stream) {
		.type = args.buf_type,
		.fmt = fmt,
		.buffers_count = req.count,
		.buffers = buffers,
	};
}

static Buffer *streamPull(int fd, Stream *st) {
	struct v4l2_plane planes[VIDEO_MAX_PLANES] = {0};
	struct v4l2_buffer buf = {
		.type = st->type,
		.memory = st->buffers[0].buffer.memory,
		.length = VIDEO_MAX_PLANES,
		.m.planes = planes,
	};

	if (0 != ioctl(fd, VIDIOC_DQBUF, &buf)) {
		if (errno != EAGAIN /* FIXME: */ && errno != EPIPE) {
			LOGF("Failed to ioctl(VIDIOC_DQBUF): %d, %s", errno, strerror(errno));
			abort();
		}

		return NULL;
	}

	Buffer *const ret = st->buffers + buf.index;

	ret->buffer = buf;
	ret->buffer.m.planes = ret->planes;
	const int planes_count = (int)ret->buffer.length;
	for (int i = 0; i < planes_count; ++i) {
		ret->planes[i] = planes[i];
	}

	ret->enqueued = 0;
	return ret;
}

static void streamPush(int fd, Stream *st, Buffer *buf) {
	if (0 != ioctl(fd, VIDIOC_QBUF, &buf->buffer)) {
		LOGF("Failed to ioctl(VIDIOC_QBUF): %d, %s", errno, strerror(errno));
		abort();
	}
	buf->enqueued = 1;
}

int main(int args, char *argv[]) {
#define JPEG_ENCODER_DEVICE "/dev/video31"
	const int fd = open(JPEG_ENCODER_DEVICE, O_RDWR | O_NONBLOCK);
	assert(fd > 0);

	struct v4l2_capability caps;
	if (0 != ioctl(fd, VIDIOC_QUERYCAP, &caps)) {
		perror("VIDIOC_QUERYCAP");
		abort();
	}

	const u32 this_device_caps = caps.capabilities & V4L2_CAP_DEVICE_CAPS
		? caps.device_caps : caps.capabilities;
	assert((V4L2_CAP_VIDEO_CAPTURE_MPLANE | V4L2_CAP_VIDEO_M2M_MPLANE) & this_device_caps);
	assert((V4L2_CAP_VIDEO_OUTPUT_MPLANE | V4L2_CAP_VIDEO_M2M_MPLANE) & this_device_caps);

	LOG("output");
	Stream output = streamStart((BufferStart){
		.fd = fd,
		.buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
		.pixelformat = V4L2_PIX_FMT_YUV420,
		.w = 1332,
		.h = 976,
		.buffers_count = 2,
		.memory = V4L2_MEMORY_MMAP,
	});

	LOG("capture");
	Stream capture = streamStart((BufferStart){
		.fd = fd,
		.buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
		.pixelformat = V4L2_PIX_FMT_JPEG,
		.w = 1332,
		.h = 976,
		.buffers_count = 2,
		.memory = V4L2_MEMORY_MMAP,
	});

	for (int frame = 0;;) {
		// Pull output (encoder input) buffer, fill it and push
		for (;;) {
			Buffer *input = streamPull(fd, &output);
			if (!input)
				break;
		}

		// Push all available for encoding
		for (int i = 0; i < output.buffers_count; ++i) {
			Buffer *input = output.buffers + i;
			if (input->enqueued)
				continue;

			//input->buffer.index = frame;
			input->buffer.timestamp.tv_usec = frame * 1000;

			frame++;
			const int planes_count = (int)input->buffer.length;
			for (int i = 0; i < planes_count; ++i) {
				struct v4l2_plane *plane = input->planes + i;
				// TODO? plane->data_offset
				memset(input->mapped[i], frame, plane->bytesused);
			}

			LOGF("Input (%d): %u, %llums", frame, input->buffer.sequence,
				(unsigned long long)input->buffer.timestamp.tv_sec * 1000 + input->buffer.timestamp.tv_usec / 1000);

			// Push data in
			streamPush(fd, &output, input);
		}

		// Pull capture (encoder result) buffer, discard it and push back
		for (;;) {
			Buffer *encoded = streamPull(fd, &capture);
			if (!encoded)
				break;

			LOGF("Encoded: %u, %llums", encoded->buffer.sequence,
				(unsigned long long)encoded->buffer.timestamp.tv_sec * 1000 + encoded->buffer.timestamp.tv_usec / 1000);

			// Return the buffer
			streamPush(fd, &capture, encoded);
		}

		usleep(1000);
	}

	return 0;
}