video-setup/dev/cli/stream_send_cli.c

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

#include "v4l2_fmt.h"
#include "transport.h"
#include "protocol.h"
#include "stream_stats.h"
#include "error.h"

#define N_BUFS            4
#define DEFAULT_PORT      7700
#define DEFAULT_STREAM_ID 1

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

/* ------------------------------------------------------------------ */
/* Transport callbacks                                                   */
/* ------------------------------------------------------------------ */

static void on_frame(struct Transport_Conn *conn,
                      struct Transport_Frame *frame,
                      void *userdata)
{
	(void)conn; (void)userdata;
	/* Receiver may send responses; just discard them. */
	free(frame->payload);
}

static void on_disconnect(struct Transport_Conn *conn, void *userdata)
{
	(void)conn; (void)userdata;
	fprintf(stderr, "stream_send_cli: disconnected from receiver\n");
}

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

static void usage(void)
{
	fprintf(stderr,
	    "usage: stream_send_cli [--device PATH] [--host HOST] [--port PORT]\n"
	    "                       [--stream-id N]\n"
	    "\n"
	    "Captures MJPEG from a V4L2 device and streams VIDEO_FRAME messages over TCP.\n"
	    "Prints frame rate and throughput to stderr every 0.5 s.\n"
	    "\n"
	    "defaults: /dev/video0  127.0.0.1  7700  stream-id=1\n");
}

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

int main(int argc, char **argv)
{
	const char  *device    = "/dev/video0";
	const char  *host      = "127.0.0.1";
	uint16_t     port      = DEFAULT_PORT;
	uint16_t     stream_id = DEFAULT_STREAM_ID;

	for (int i = 1; i < argc; i++) {
		if (strcmp(argv[i], "--device") == 0 && i + 1 < argc) {
			device = argv[++i];
		} else if (strcmp(argv[i], "--host") == 0 && i + 1 < argc) {
			host = argv[++i];
		} else if (strcmp(argv[i], "--port") == 0 && i + 1 < argc) {
			port = (uint16_t)atoi(argv[++i]);
		} else if (strcmp(argv[i], "--stream-id") == 0 && i + 1 < argc) {
			stream_id = (uint16_t)atoi(argv[++i]);
		} else {
			usage(); 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 (v4l2_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 — MJPEG, best FPS then largest size              */
	/* ---------------------------------------------------------------- */

	V4l2_Fmt_Option opts[V4L2_FMT_MAX_OPTS];
	int n = v4l2_enumerate_formats(fd, opts, V4L2_FMT_MAX_OPTS, V4L2_PIX_FMT_MJPEG);
	if (n == 0) {
		fprintf(stderr, "%s: no MJPEG formats found\n", device);
		close(fd); return 1;
	}
	const V4l2_Fmt_Option *best = v4l2_select_best(opts, n);

	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 (v4l2_xioctl(fd, VIDIOC_S_FMT, &fmt) < 0) {
		perror("VIDIOC_S_FMT"); close(fd); return 1;
	}

	int width  = (int)fmt.fmt.pix.width;
	int height = (int)fmt.fmt.pix.height;
	int fps_n  = best->fps_n;
	int fps_d  = best->fps_d;

	{
		struct v4l2_streamparm parm = {0};
		parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		parm.parm.capture.timeperframe.numerator   = (uint32_t)fps_d;
		parm.parm.capture.timeperframe.denominator = (uint32_t)fps_n;
		v4l2_xioctl(fd, VIDIOC_S_PARM, &parm);
		if (v4l2_xioctl(fd, VIDIOC_G_PARM, &parm) == 0 &&
		    parm.parm.capture.timeperframe.denominator > 0) {
			fps_n = (int)parm.parm.capture.timeperframe.denominator;
			fps_d = (int)parm.parm.capture.timeperframe.numerator;
		}
	}

	fprintf(stderr, "device:  %s (%s)\n", device, (char *)cap.card);
	fprintf(stderr, "format:  MJPEG  %dx%d  target=%.1f fps\n",
	        width, height,
	        fps_d > 0 ? (double)fps_n / 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 (v4l2_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 (v4l2_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 (v4l2_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 (v4l2_xioctl(fd, VIDIOC_STREAMON, &stream_type) < 0) {
		perror("VIDIOC_STREAMON"); close(fd); return 1;
	}

	/* ---------------------------------------------------------------- */
	/* Connect to receiver                                               */
	/* ---------------------------------------------------------------- */

	struct Transport_Conn *conn = NULL;
	struct App_Error err = transport_connect(&conn, host, port,
	                                          TRANSPORT_DEFAULT_MAX_PAYLOAD,
	                                          on_frame, on_disconnect, NULL);
	if (!APP_IS_OK(err)) {
		app_error_print(&err);
		v4l2_xioctl(fd, VIDIOC_STREAMOFF, &stream_type);
		close(fd); return 1;
	}

	fprintf(stderr, "connected to %s:%u  stream_id=%u\n", host, port, stream_id);

	err = proto_write_stream_open(conn, 1 /* request_id */, stream_id,
	                               PROTO_FORMAT_MJPEG,
	                               0 /* pixel_format: compressed */,
	                               PROTO_ORIGIN_DEVICE_NATIVE);
	if (!APP_IS_OK(err)) {
		app_error_print(&err);
		transport_conn_close(conn);
		v4l2_xioctl(fd, VIDIOC_STREAMOFF, &stream_type);
		close(fd); return 1;
	}

	/* ---------------------------------------------------------------- */
	/* Capture + send loop                                               */
	/* ---------------------------------------------------------------- */

	Stream_Stats stats;
	stream_stats_init(&stats, stream_id);

	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, "stream_send_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 (v4l2_xioctl(fd, VIDIOC_DQBUF, &buf) < 0) {
			if (errno == EAGAIN) { continue; }
			perror("VIDIOC_DQBUF"); break;
		}

		const uint8_t *data   = (const uint8_t *)bufs[buf.index].start;
		uint32_t       nbytes = buf.bytesused;

		err = proto_write_video_frame(conn, stream_id, data, nbytes);

		if (v4l2_xioctl(fd, VIDIOC_QBUF, &buf) < 0) {
			perror("VIDIOC_QBUF");
			if (!APP_IS_OK(err)) { app_error_print(&err); }
			break;
		}

		if (!APP_IS_OK(err)) {
			app_error_print(&err);
			break;
		}

		stream_stats_record_frame(&stats, nbytes);
		if (stream_stats_tick(&stats)) {
			fprintf(stderr, "stream %u  %.1f fps  %.2f Mbps\n",
			        stream_id, stats.fps, stats.mbps);
		}
	}

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

	transport_conn_close(conn);
	v4l2_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);
		}
	}
	close(fd);
	return 0;
}