video-setup/dev/cli/v4l2_view_cli.c

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <time.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/select.h>
#include <linux/videodev2.h>

#include "xorg.h"

#define N_BUFS    4
#define MAX_OPTS  1024

/* ------------------------------------------------------------------ */
/* Format option — one (pixfmt, size, fps) combination                 */
/* ------------------------------------------------------------------ */

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

/* fps_a > fps_b ? */
static int fps_gt(const Fmt_Option *a, const Fmt_Option *b)
{
	return (long long)a->fps_n * b->fps_d > (long long)b->fps_n * a->fps_d;
}

static int fps_eq(const Fmt_Option *a, const Fmt_Option *b)
{
	return (long long)a->fps_n * b->fps_d == (long long)b->fps_n * a->fps_d;
}

/* ------------------------------------------------------------------ */
/* Format enumeration                                                   */
/* ------------------------------------------------------------------ */

typedef struct {
	Fmt_Option *opts;
	int         n;
	int         max;
} Opt_List;

static void opt_push(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++] = (Fmt_Option){ pixfmt, w, h, fps_n, fps_d };
}

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

static void collect_intervals(int fd, uint32_t pixfmt, int w, int h, 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; xioctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &fie) == 0; fie.index++) {
		if (fie.type == V4L2_FRMIVAL_TYPE_DISCRETE) {
			opt_push(l, pixfmt, w, h,
			         (int)fie.discrete.denominator,
			         (int)fie.discrete.numerator);
		} else {
			/* Stepwise/continuous: record the fastest (minimum) interval. */
			opt_push(l, pixfmt, w, h,
			         (int)fie.stepwise.min.denominator,
			         (int)fie.stepwise.min.numerator);
			break;
		}
	}
}

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

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

/*
 * Enumerate all (pixfmt, size, fps) combinations the device supports.
 * Filtered to formats we can handle (MJPEG, YUYV).
 * If fmt_filter is non-zero, only that pixel format is considered.
 */
static int enumerate_formats(int fd, Fmt_Option *buf, int buf_max,
                               uint32_t fmt_filter)
{
	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;
	     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; }
		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 for equal FPS and area
 */
static const Fmt_Option *select_best(const Fmt_Option *opts, int n)
{
	if (n == 0) { return NULL; }
	const Fmt_Option *best = &opts[0];
	for (int i = 1; i < n; i++) {
		const Fmt_Option *o = &opts[i];
		if (fps_gt(o, best)) {
			best = o;
		} else if (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;
}

/* ------------------------------------------------------------------ */
/* YUYV → planar YUV420 conversion                                     */
/* ------------------------------------------------------------------ */

static void yuyv_to_yuv420(const uint8_t *yuyv, int stride,
                             int w, int h,
                             uint8_t *y_out, uint8_t *cb_out, uint8_t *cr_out)
{
	for (int row = 0; row < h; row++) {
		const uint8_t *src = yuyv + row * stride;
		uint8_t       *dst = y_out + row * w;
		for (int col = 0; col < w; col++) {
			dst[col] = src[col * 2];
		}
	}
	for (int row = 0; row < h; row += 2) {
		const uint8_t *src = yuyv + row * stride;
		int c_row = row / 2;
		for (int col = 0; col < w; col += 2) {
			cb_out[c_row * (w / 2) + col / 2] = src[col * 2 + 1];
			cr_out[c_row * (w / 2) + col / 2] = src[col * 2 + 3];
		}
	}
}

/* ------------------------------------------------------------------ */
/* Mmap buffers                                                         */
/* ------------------------------------------------------------------ */

typedef struct { void *start; size_t length; } Mmap_Buf;

/* ------------------------------------------------------------------ */
/* Usage                                                                */
/* ------------------------------------------------------------------ */

static void usage(void)
{
	fprintf(stderr,
	    "usage: v4l2_view_cli [--device PATH]\n"
	    "                     [--width N --height N]\n"
	    "                     [--format mjpeg|yuyv]\n"
	    "                     [--scale stretch|fit|fill|1:1]\n"
	    "                     [--anchor center|topleft]\n"
	    "                     [--x N] [--y N]\n"
	    "\n"
	    "Opens a V4L2 capture device and displays the live feed.\n"
	    "Without --width/--height, selects the highest-FPS mode\n"
	    "and within that the largest resolution.\n"
	    "Q or Escape closes the window.\n"
	    "\n"
	    "defaults: /dev/video0  auto  fit center at 0,0\n");
}

/* ------------------------------------------------------------------ */
/* Main                                                                 */
/* ------------------------------------------------------------------ */

int main(int argc, char **argv)
{
	const char  *device     = "/dev/video0";
	int          req_width  = 0;
	int          req_height = 0;
	int          win_x      = 0;
	int          win_y      = 0;
	Xorg_Scale   scale      = XORG_SCALE_FIT;
	Xorg_Anchor  anchor     = XORG_ANCHOR_CENTER;
	uint32_t     fmt_filter = 0;  /* 0 = auto */

	for (int i = 1; i < argc; i++) {
		if (strcmp(argv[i], "--device") == 0 && i + 1 < argc) {
			device = argv[++i];
		} else if (strcmp(argv[i], "--width") == 0 && i + 1 < argc) {
			req_width = atoi(argv[++i]);
		} else if (strcmp(argv[i], "--height") == 0 && i + 1 < argc) {
			req_height = atoi(argv[++i]);
		} else if (strcmp(argv[i], "--x") == 0 && i + 1 < argc) {
			win_x = atoi(argv[++i]);
		} else if (strcmp(argv[i], "--y") == 0 && i + 1 < argc) {
			win_y = atoi(argv[++i]);
		} else if (strcmp(argv[i], "--format") == 0 && i + 1 < argc) {
			i++;
			if      (strcmp(argv[i], "mjpeg") == 0) { fmt_filter = V4L2_PIX_FMT_MJPEG; }
			else if (strcmp(argv[i], "yuyv")  == 0) { fmt_filter = V4L2_PIX_FMT_YUYV;  }
			else { fprintf(stderr, "unknown format: %s\n", argv[i]); usage(); return 1; }
		} else if (strcmp(argv[i], "--scale") == 0 && i + 1 < argc) {
			i++;
			if      (strcmp(argv[i], "stretch") == 0) { scale = XORG_SCALE_STRETCH; }
			else if (strcmp(argv[i], "fit")     == 0) { scale = XORG_SCALE_FIT;     }
			else if (strcmp(argv[i], "fill")    == 0) { scale = XORG_SCALE_FILL;    }
			else if (strcmp(argv[i], "1:1")     == 0) { scale = XORG_SCALE_1_1;     }
			else { fprintf(stderr, "unknown scale: %s\n", argv[i]); usage(); return 1; }
		} else if (strcmp(argv[i], "--anchor") == 0 && i + 1 < argc) {
			i++;
			if      (strcmp(argv[i], "center")  == 0) { anchor = XORG_ANCHOR_CENTER;   }
			else if (strcmp(argv[i], "topleft") == 0) { anchor = XORG_ANCHOR_TOP_LEFT; }
			else { fprintf(stderr, "unknown anchor: %s\n", argv[i]); usage(); return 1; }
		} else {
			usage(); return 1;
		}
	}

	if (!xorg_available()) {
		fprintf(stderr, "v4l2_view_cli: built without HAVE_GLFW — viewer not available\n");
		return 1;
	}

	/* ---------------------------------------------------------------- */
	/* Open V4L2 device                                                  */
	/* ---------------------------------------------------------------- */

	int fd = open(device, O_RDWR | O_NONBLOCK);
	if (fd < 0) { perror(device); return 1; }

	struct v4l2_capability cap = {0};
	if (xioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
		perror("VIDIOC_QUERYCAP"); close(fd); return 1;
	}
	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
		fprintf(stderr, "%s: not a capture device\n", device); close(fd); return 1;
	}
	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
		fprintf(stderr, "%s: does not support streaming\n", device); close(fd); return 1;
	}

	/* ---------------------------------------------------------------- */
	/* Format selection                                                   */
	/* ---------------------------------------------------------------- */

	int      width, height, stride;
	int      use_mjpeg;
	int      sel_fps_n, sel_fps_d;

	if (req_width > 0 && req_height > 0) {
		/*
		 * Explicit size requested — skip enumeration, negotiate directly.
		 * Try MJPEG first (or whatever fmt_filter says), fall back to YUYV.
		 */
		struct v4l2_format fmt = {0};
		fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		fmt.fmt.pix.width       = (uint32_t)req_width;
		fmt.fmt.pix.height      = (uint32_t)req_height;
		fmt.fmt.pix.field       = V4L2_FIELD_ANY;

		use_mjpeg = 0;
		if (fmt_filter != V4L2_PIX_FMT_YUYV) {
			fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
			if (xioctl(fd, VIDIOC_S_FMT, &fmt) == 0 &&
			    fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG) {
				use_mjpeg = 1;
			}
		}
		if (!use_mjpeg) {
			fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
			if (xioctl(fd, VIDIOC_S_FMT, &fmt) < 0 ||
			    fmt.fmt.pix.pixelformat != V4L2_PIX_FMT_YUYV) {
				fprintf(stderr, "%s: could not set %dx%d in MJPEG or YUYV\n",
				        device, req_width, req_height);
				close(fd); return 1;
			}
		}
		width  = (int)fmt.fmt.pix.width;
		height = (int)fmt.fmt.pix.height;
		stride = (int)fmt.fmt.pix.bytesperline;
		sel_fps_n = 0; sel_fps_d = 1;  /* unknown until G_PARM below */
	} else {
		/* Enumerate all supported modes and pick the best. */
		Fmt_Option *opts = malloc(MAX_OPTS * sizeof(*opts));
		if (!opts) { fprintf(stderr, "out of memory\n"); close(fd); return 1; }

		int n = enumerate_formats(fd, opts, MAX_OPTS, fmt_filter);
		if (n == 0) {
			fprintf(stderr, "%s: no usable formats found (MJPEG/YUYV)\n", device);
			free(opts); close(fd); return 1;
		}

		const Fmt_Option *best = select_best(opts, n);

		/* Apply the selected format. */
		struct v4l2_format fmt = {0};
		fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		fmt.fmt.pix.pixelformat = best->pixfmt;
		fmt.fmt.pix.width       = (uint32_t)best->w;
		fmt.fmt.pix.height      = (uint32_t)best->h;
		fmt.fmt.pix.field       = V4L2_FIELD_ANY;
		if (xioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
			perror("VIDIOC_S_FMT"); free(opts); close(fd); return 1;
		}

		use_mjpeg  = (fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_MJPEG);
		width      = (int)fmt.fmt.pix.width;
		height     = (int)fmt.fmt.pix.height;
		stride     = (int)fmt.fmt.pix.bytesperline;
		sel_fps_n  = best->fps_n;
		sel_fps_d  = best->fps_d;
		free(opts);
	}

	/* Request the selected frame rate (driver may ignore, but try). */
	{
		struct v4l2_streamparm parm = {0};
		parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		parm.parm.capture.timeperframe.numerator   = (uint32_t)sel_fps_d;
		parm.parm.capture.timeperframe.denominator = (uint32_t)sel_fps_n;
		xioctl(fd, VIDIOC_S_PARM, &parm);
		/* Read back what the driver actually set. */
		if (xioctl(fd, VIDIOC_G_PARM, &parm) == 0 &&
		    parm.parm.capture.timeperframe.denominator > 0) {
			sel_fps_n = (int)parm.parm.capture.timeperframe.denominator;
			sel_fps_d = (int)parm.parm.capture.timeperframe.numerator;
		}
	}

	printf("device:  %s (%s)\n", device, (char *)cap.card);
	printf("format:  %s  %dx%d  stride=%d  target=%.1f fps\n",
	       use_mjpeg ? "MJPEG" : "YUYV", width, height, stride,
	       sel_fps_d > 0 ? (double)sel_fps_n / sel_fps_d : 0.0);

	/* ---------------------------------------------------------------- */
	/* Mmap buffers + stream on                                          */
	/* ---------------------------------------------------------------- */

	struct v4l2_requestbuffers req = {0};
	req.count  = N_BUFS;
	req.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = V4L2_MEMORY_MMAP;
	if (xioctl(fd, VIDIOC_REQBUFS, &req) < 0) {
		perror("VIDIOC_REQBUFS"); close(fd); return 1;
	}

	Mmap_Buf bufs[N_BUFS] = {0};
	for (unsigned i = 0; i < req.count; i++) {
		struct v4l2_buffer buf = {0};
		buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index  = i;
		if (xioctl(fd, VIDIOC_QUERYBUF, &buf) < 0) {
			perror("VIDIOC_QUERYBUF"); close(fd); return 1;
		}
		bufs[i].length = buf.length;
		bufs[i].start  = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
		                      MAP_SHARED, fd, buf.m.offset);
		if (bufs[i].start == MAP_FAILED) {
			perror("mmap"); close(fd); return 1;
		}
	}

	for (unsigned i = 0; i < req.count; i++) {
		struct v4l2_buffer buf = {0};
		buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		buf.index  = i;
		if (xioctl(fd, VIDIOC_QBUF, &buf) < 0) {
			perror("VIDIOC_QBUF"); close(fd); return 1;
		}
	}

	enum v4l2_buf_type stream_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (xioctl(fd, VIDIOC_STREAMON, &stream_type) < 0) {
		perror("VIDIOC_STREAMON"); close(fd); return 1;
	}

	/* ---------------------------------------------------------------- */
	/* Open viewer                                                        */
	/* ---------------------------------------------------------------- */

	Xorg_Viewer *v = xorg_viewer_open(win_x, win_y, width, height, "v4l2_view_cli");
	if (!v) {
		fprintf(stderr, "v4l2_view_cli: failed to open viewer window\n");
		xioctl(fd, VIDIOC_STREAMOFF, &stream_type);
		close(fd); return 1;
	}
	xorg_viewer_set_scale(v, scale);
	xorg_viewer_set_anchor(v, anchor);

	/* ---------------------------------------------------------------- */
	/* YUYV conversion buffer                                             */
	/* ---------------------------------------------------------------- */

	uint8_t *yuv420_buf = NULL;
	if (!use_mjpeg) {
		yuv420_buf = malloc((size_t)(width * height * 3 / 2));
		if (!yuv420_buf) {
			fprintf(stderr, "v4l2_view_cli: out of memory\n");
			xorg_viewer_close(v);
			xioctl(fd, VIDIOC_STREAMOFF, &stream_type);
			close(fd); return 1;
		}
	}

	/* ---------------------------------------------------------------- */
	/* Capture loop                                                       */
	/* ---------------------------------------------------------------- */

	struct timespec t_fps;
	clock_gettime(CLOCK_MONOTONIC, &t_fps);
	int   fps_frame_count = 0;
	float displayed_fps   = 0.0f;

	/* Set initial info overlay; fps will be filled in once measured. */
	const char *fmt_name = use_mjpeg ? "MJPEG" : "YUYV";
	{
		char info[64];
		snprintf(info, sizeof(info), "%s %dx%d @ --.- fps", fmt_name, width, height);
		xorg_viewer_set_overlay_text(v, 0, 10, 10, info, 1.0f, 1.0f, 0.8f);
	}

	while (1) {
		fd_set fds;
		FD_ZERO(&fds);
		FD_SET(fd, &fds);
		struct timeval tv = {1, 0};
		int r = select(fd + 1, &fds, NULL, NULL, &tv);

		if (r < 0) {
			if (errno == EINTR) { continue; }
			perror("select"); break;
		}
		if (r == 0) {
			fprintf(stderr, "v4l2_view_cli: select timeout — no frames\n");
			continue;
		}

		struct v4l2_buffer buf = {0};
		buf.type   = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buf.memory = V4L2_MEMORY_MMAP;
		if (xioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
			if (errno == EAGAIN) { continue; }
			perror("VIDIOC_DQBUF"); break;
		}

		const uint8_t *data = bufs[buf.index].start;

		if (use_mjpeg) {
			xorg_viewer_push_mjpeg(v, data, buf.bytesused);
		} else {
			uint8_t *y_p  = yuv420_buf;
			uint8_t *cb_p = y_p  + width * height;
			uint8_t *cr_p = cb_p + width * height / 4;
			yuyv_to_yuv420(data, stride, width, height, y_p, cb_p, cr_p);
			xorg_viewer_push_yuv420(v, y_p, cb_p, cr_p, width, height);
		}

		if (xioctl(fd, VIDIOC_QBUF, &buf) < 0) {
			perror("VIDIOC_QBUF"); break;
		}

		/* Update FPS overlay every 0.5s. */
		fps_frame_count++;
		struct timespec now;
		clock_gettime(CLOCK_MONOTONIC, &now);
		double elapsed = (now.tv_sec  - t_fps.tv_sec) +
		                 (now.tv_nsec - t_fps.tv_nsec) * 1e-9;
		if (elapsed >= 0.5) {
			displayed_fps   = (float)(fps_frame_count / elapsed);
			fps_frame_count = 0;
			t_fps           = now;
			char info[64];
			snprintf(info, sizeof(info), "%s %dx%d @ %.1f fps",
			         fmt_name, width, height, displayed_fps);
			xorg_viewer_set_overlay_text(v, 0, 10, 10, info, 1.0f, 1.0f, 0.8f);
		}

		if (!xorg_viewer_handle_events(v)) { break; }
	}

	/* ---------------------------------------------------------------- */
	/* Cleanup                                                            */
	/* ---------------------------------------------------------------- */

	xorg_viewer_close(v);
	xioctl(fd, VIDIOC_STREAMOFF, &stream_type);
	for (unsigned i = 0; i < req.count; i++) {
		if (bufs[i].start && bufs[i].start != MAP_FAILED) {
			munmap(bufs[i].start, bufs[i].length);
		}
	}
	free(yuv420_buf);
	close(fd);
	return 0;
}