video-setup/include/v4l2_fmt.h

#pragma once

/*
 * Header-only V4L2 format enumeration.
 * Enumerates (pixfmt, size, fps) combinations for MJPEG/YUYV capture devices.
 *
 * Usage:
 *   V4l2_Fmt_Option opts[V4L2_FMT_MAX_OPTS];
 *   int n = v4l2_enumerate_formats(fd, opts, V4L2_FMT_MAX_OPTS, 0);
 *   const V4l2_Fmt_Option *best = v4l2_select_best(opts, n);
 */

#include <stdint.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>

#define V4L2_FMT_MAX_OPTS  1024

typedef struct {
	uint32_t pixfmt;
	int      w, h;
	int      fps_n;   /* fps = fps_n / fps_d */
	int      fps_d;
} V4l2_Fmt_Option;

static inline int v4l2_xioctl(int fd, unsigned long req, void *arg)
{
	int r;
	do { r = ioctl(fd, req, arg); } while (r == -1 && errno == EINTR);
	return r;
}

static inline int v4l2_fmt_fps_gt(const V4l2_Fmt_Option *a, const V4l2_Fmt_Option *b)
{
	return (long long)a->fps_n * b->fps_d > (long long)b->fps_n * a->fps_d;
}

static inline int v4l2_fmt_fps_eq(const V4l2_Fmt_Option *a, const V4l2_Fmt_Option *b)
{
	return (long long)a->fps_n * b->fps_d == (long long)b->fps_n * a->fps_d;
}

typedef struct {
	V4l2_Fmt_Option *opts;
	int              n;
	int              max;
} V4l2_Opt_List;

static inline void v4l2_opt_push(V4l2_Opt_List *l, uint32_t pixfmt,
                                   int w, int h, int fps_n, int fps_d)
{
	if (l->n >= l->max) { return; }
	l->opts[l->n++] = (V4l2_Fmt_Option){ pixfmt, w, h, fps_n, fps_d };
}

static inline void v4l2_collect_intervals(int fd, uint32_t pixfmt, int w, int h,
                                            V4l2_Opt_List *l)
{
	struct v4l2_frmivalenum fie = {0};
	fie.pixel_format = pixfmt;
	fie.width        = (uint32_t)w;
	fie.height       = (uint32_t)h;

	for (fie.index = 0;
	     v4l2_xioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &fie) == 0;
	     fie.index++) {
		if (fie.type == V4L2_FRMIVAL_TYPE_DISCRETE) {
			v4l2_opt_push(l, pixfmt, w, h,
			              (int)fie.discrete.denominator,
			              (int)fie.discrete.numerator);
		} else {
			/* Stepwise/continuous: record the fastest (minimum) interval. */
			v4l2_opt_push(l, pixfmt, w, h,
			              (int)fie.stepwise.min.denominator,
			              (int)fie.stepwise.min.numerator);
			break;
		}
	}
}

static inline void v4l2_collect_sizes(int fd, uint32_t pixfmt, V4l2_Opt_List *l)
{
	struct v4l2_frmsizeenum fse = {0};
	fse.pixel_format = pixfmt;

	for (fse.index = 0;
	     v4l2_xioctl(fd, VIDIOC_ENUM_FRAMESIZES, &fse) == 0;
	     fse.index++) {
		if (fse.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
			v4l2_collect_intervals(fd, pixfmt,
			                       (int)fse.discrete.width,
			                       (int)fse.discrete.height, l);
		} else {
			/* Stepwise/continuous: only probe the maximum size. */
			v4l2_collect_intervals(fd, pixfmt,
			                       (int)fse.stepwise.max_width,
			                       (int)fse.stepwise.max_height, l);
			break;
		}
	}
}

/*
 * Enumerate all (pixfmt, size, fps) combos the device supports.
 * Filtered to MJPEG and YUYV. fmt_filter=0 accepts both.
 * Returns the count written to buf.
 */
static inline int v4l2_enumerate_formats(int fd, V4l2_Fmt_Option *buf, int buf_max,
                                           uint32_t fmt_filter)
{
	V4l2_Opt_List l = { buf, 0, buf_max };

	struct v4l2_fmtdesc fd_desc = {0};
	fd_desc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

	for (fd_desc.index = 0;
	     v4l2_xioctl(fd, VIDIOC_ENUM_FMT, &fd_desc) == 0;
	     fd_desc.index++) {
		uint32_t pf = fd_desc.pixelformat;
		if (pf != V4L2_PIX_FMT_MJPEG && pf != V4L2_PIX_FMT_YUYV) { continue; }
		if (fmt_filter && pf != fmt_filter) { continue; }
		v4l2_collect_sizes(fd, pf, &l);
	}
	return l.n;
}

/*
 * Select the best option from the list:
 *   1. Highest FPS
 *   2. Largest area (w×h) among equal-FPS entries
 *   3. MJPEG preferred over YUYV on equal FPS and area
 */
static inline const V4l2_Fmt_Option *v4l2_select_best(const V4l2_Fmt_Option *opts, int n)
{
	if (n == 0) { return NULL; }
	const V4l2_Fmt_Option *best = &opts[0];
	for (int i = 1; i < n; i++) {
		const V4l2_Fmt_Option *o = &opts[i];
		if (v4l2_fmt_fps_gt(o, best)) {
			best = o;
		} else if (v4l2_fmt_fps_eq(o, best)) {
			int o_area = o->w * o->h;
			int b_area = best->w * best->h;
			if (o_area > b_area) {
				best = o;
			} else if (o_area == b_area &&
			           o->pixfmt == V4L2_PIX_FMT_MJPEG &&
			           best->pixfmt != V4L2_PIX_FMT_MJPEG) {
				best = o;
			}
		}
	}
	return best;
}