libmegapixels/util/getframe.c

#include <libmegapixels.h>
#include <stdio.h>
#include <limits.h>
#include <string.h>
#include <linux/videodev2.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <sys/types.h>
#include <errno.h>
#include <sys/mman.h>
#include <getopt.h>
#include <ctype.h>

struct buffer {
		void *start;
		size_t length;
};
struct buffer *buffers;

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

int
main(int argc, char *argv[])
{
	int c;
	int camera_id = 0;
	long res;
	char *end;
	char *outfile = NULL;

	while ((c = getopt(argc, argv, "c:o:")) != -1) {
		switch (c) {
			case 'c':
				res = strtol(optarg, &end, 10);
				if (end == optarg || end == NULL || *end != '\0') {
					fprintf(stderr, "Invalid number for -c\n");
					return 1;
				}
				camera_id = (int) res;
				break;
			case 'o':
				outfile = optarg;
				break;
			case '?':
				if (optopt == 'd' || optopt == 'l') {
					fprintf(stderr, "Option -%c requires an argument.\n", optopt);
				} else if (isprint(optopt)) {
					fprintf(stderr, "Unknown option '-%c'\n", optopt);
				} else {
					fprintf(stderr, "Unknown option character x%x\n", optopt);
				}
				return 1;
			default:
				return 1;
		}
	}


	char configpath[PATH_MAX];
	int ret = libmegapixels_find_config(configpath);
	libmegapixels_devconfig *config = {0};
	if (ret) {
		printf("Using config: %s\n", configpath);
		libmegapixels_load_file(&config, configpath);
	} else {
		fprintf(stderr, "No config found for this device\n");
		return 1;
	}

	if (config->count == 0) {
		fprintf(stderr, "No valid camera configuration\n");
		return 1;
	}

	if (camera_id > config->count) {
		fprintf(stderr, "Camera id %d does not exist\n", camera_id);
	}

	libmegapixels_camera *camera = config->cameras[camera_id];
	if (libmegapixels_open(camera) != 0) {
		fprintf(stderr, "Could not open default camera\n");
		return 1;
	}

	libmegapixels_mode *mode = camera->modes[0];
	unsigned int frame_size = libmegapixels_select_mode(camera, mode);
	if (frame_size == 0) {
		fprintf(stderr, "Could not select mode\n");
		return 1;
	}

	// Do the reqular V4L2 stuff to get a frame
	struct v4l2_capability cap;
	if (ioctl(camera->video_fd, VIDIOC_QUERYCAP, &cap) != 0) {
		fprintf(stderr, "VIDIOC_QUERYCAP failed\n");
		return 1;
	}
	if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
		fprintf(stderr, "Device does not support streaming i/o\n");
		return 1;
	}

	struct v4l2_requestbuffers req = {0};
	req.count = 4;
	req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	req.memory = V4L2_MEMORY_MMAP;
	if (xioctl(camera->video_fd, VIDIOC_REQBUFS, &req) == -1) {
		fprintf(stderr, "VIDIOC_REQBUFS failed\n");
		return 1;
	}
	buffers = calloc(req.count, sizeof(*buffers));
	for (int 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(camera->video_fd, VIDIOC_QUERYBUF, &buf) == -1) {
			fprintf(stderr, "VIDIOC_QUERYBUF failed\n");
			return 1;
		}

		buffers[i].length = buf.length;
		buffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, camera->video_fd, buf.m.offset);
		if (buffers[i].start == MAP_FAILED) {
			fprintf(stderr, "mmap() failed\n");
			return 1;
		}
	}

	for (int i = 0; i < req.count; i++) {
		struct v4l2_buffer qbuf = {0};
		qbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		qbuf.memory = V4L2_MEMORY_MMAP;
		qbuf.index = i;

		if (xioctl(camera->video_fd, VIDIOC_QBUF, &qbuf) == -1) {
			fprintf(stderr, "VIDIOC_QBUF failed\n");
			return 1;
		}
	}
	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	if (xioctl(camera->video_fd, VIDIOC_STREAMON, &type) == -1) {
		fprintf(stderr, "VIDIOC_STREAMON failed\n");
		return 1;
	}


	int count = 5;
	while (count-- > 0) {
		while (1) {
			fd_set(fds);
			FD_ZERO(&fds);
			FD_SET(camera->video_fd, &fds);

			int sr = select(FD_SETSIZE, &fds, NULL, NULL, NULL);
			if (sr == -1) {
				if (errno == EINTR) {
					count++;
					continue;
				}
				fprintf(stderr, "select() failed: %s\n", strerror(errno));
				return 1;
			}


			struct v4l2_buffer buf = {0};
			buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
			buf.memory = V4L2_MEMORY_MMAP;
			if (xioctl(camera->video_fd, VIDIOC_DQBUF, &buf) == -1) {
				fprintf(stderr, "VIDIOC_DQBUF failed\n");
				return 1;
			}

			fprintf(stderr, "received frame\n");

			if (count == 1 && outfile != NULL) {
				FILE *fp = fopen(outfile, "w");
				fwrite(buffers[buf.index].start, buf.bytesused, 1, fp);
				fclose(fp);
				printf("Stored frame to: %s\n", outfile);
				printf("Format: %dx%x\n", mode->width, mode->height);

				char fourcc[5] = {0};
				fourcc[0] = (char) (mode->v4l_pixfmt & 0xff);
				fourcc[1] = (char) ((mode->v4l_pixfmt >> 8) & 0xff);
				fourcc[2] = (char) ((mode->v4l_pixfmt >> 16) & 0xff);
				fourcc[3] = (char) ((mode->v4l_pixfmt >> 24) & 0xff);
				printf("Pixfmt: %s\n", fourcc);
			}

			if (xioctl(camera->video_fd, VIDIOC_QBUF, &buf) == -1) {
				fprintf(stderr, "VIDIOC_DQBUF failed\n");
				return 1;
			}
			break;
		}
	}
	return 0;
}