v4l2-test

v4l2-test.c

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <errno.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>

#include <linux/videodev2.h>

void print_video_inputs(int fd) {
	struct v4l2_input input;

	memset(&input, 0, sizeof(input));
	for(input.index = 0; ioctl(fd, VIDIOC_ENUMINPUT, &input) >= 0; input.index++) {
		printf("Video Input %d: %s\n", input.index, input.name);
	}
}
void switch_video_input(int fd, int index) {
	if(ioctl(fd, VIDIOC_S_INPUT, &index) < 0) {
		perror ("VIDIOC_S_INPUT");
		exit (EXIT_FAILURE);
	}
}
void print_video_standards(int fd) {
	struct v4l2_standard std;

	memset(&std, 0, sizeof(std));
	for(std.index = 0; ioctl(fd, VIDIOC_ENUMSTD, &std) >= 0; std.index++) {
		printf("Video Standard %d: %s [%d]\n", std.index, std.name, std.id);
	}
}
void print_controls(int fd) {
	struct v4l2_queryctrl queryctrl;

	memset(&queryctrl, 0, sizeof(queryctrl));
	for(queryctrl.id = V4L2_CID_BASE; ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl) >= 0 && queryctrl.id < V4L2_CID_LASTP1; queryctrl.id++) {
		printf("Control %x: %s\n", queryctrl.id, queryctrl.name);
	}
}
void print_extended_controls(int fd) {
	struct v4l2_queryctrl queryctrl;

	memset(&queryctrl, 0, sizeof(queryctrl));
	for(queryctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL; ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl) >= 0; queryctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL) {
		printf("Extended Control %x: %s\n", queryctrl.id, queryctrl.name);
	}
}
void print_formats(int fd) {
	struct v4l2_fmtdesc fmt;

	memset(&fmt, 0, sizeof(fmt));
	fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	for(fmt.index = 0; ioctl(fd, VIDIOC_ENUM_FMT, &fmt) >= 0; fmt.index++) {
		printf("Format %d: %s %x\n", fmt.index, fmt.description, fmt.pixelformat);
	}

}
void print_caps(int fd) {
	struct v4l2_capability cap;
	
	if(ioctl(fd, VIDIOC_QUERYCAP, &cap) < 0) {
		perror("VIDIOC_QUERYCAP");
	}

	printf("%s %s %s %d %x\n", cap.driver, cap.card, cap.bus_info, cap.version, cap.capabilities);
}

struct buffers {
	int size;
	struct {
		void *start;
		size_t length;
		struct v4l2_buffer buffer;
	} *bufs;
};

struct buffers* init_buffers(int fd, int num) {
	int i = 0;
	struct v4l2_requestbuffers reqbuf;
	memset (&reqbuf, 0, sizeof (reqbuf));
	reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	reqbuf.memory = V4L2_MEMORY_MMAP;
	reqbuf.count = num;

	struct buffers* ret = NULL;
	ret = malloc(sizeof(struct buffers));
	if(ret == NULL) {
		return NULL;
	}
	if(ioctl(fd, VIDIOC_REQBUFS, &reqbuf)<0) {
		perror("VIDIOC_REQBUFS");
		free(ret);
		return NULL;
	}
	ret->size = reqbuf.count;
	ret->bufs = calloc(ret->size, sizeof(*(ret->bufs)));
	if(ret->bufs == NULL) {
		reqbuf.count = 0;
		ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
		free(ret);
		return NULL;
	}

	for(i=0; i < ret->size; i++) {
		memset(&(ret->bufs[i].buffer), 0, sizeof(struct v4l2_buffer));
		ret->bufs[i].buffer.type = reqbuf.type;
		ret->bufs[i].buffer.memory = V4L2_MEMORY_MMAP;
		ret->bufs[i].buffer.index = i;

		if (ioctl(fd, VIDIOC_QUERYBUF, &(ret->bufs[i].buffer)) < 0) {
			perror("VIDIOC_QUERYBUF");
			break;
		}
		ret->bufs[i].length = ret->bufs[i].buffer.length;
		ret->bufs[i].start = mmap(NULL, ret->bufs[i].buffer.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, ret->bufs[i].buffer.m.offset);
		if (MAP_FAILED == ret->bufs[i].start) {
			perror("mmap");
			break;
		}
	}
	if(i < ret->size) {
		for(; i > 0; --i) {
			munmap(ret->bufs[i-1].start, ret->bufs[i-1].length);
		}
		reqbuf.count = 0;
		ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
		free(ret->bufs);
		free(ret);
		return NULL;
	}

	return ret;
}
void free_buffers(int fd, struct buffers* b) {
	int i = 0;
	for(i = 0; i < b->size; ++i) {
		munmap(b->bufs[i].start, b->bufs[i].length);
	}
	struct v4l2_requestbuffers reqbuf;
	memset (&reqbuf, 0, sizeof (reqbuf));
	reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	reqbuf.memory = V4L2_MEMORY_MMAP;
	reqbuf.count = 0;
	ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
	free(b->bufs);
	free(b);
}
int queue_all_buffers(int fd, struct buffers* b) {
	int i = 0;
	for(i = 0; i < b->size; ++i) {
		if(ioctl(fd,VIDIOC_QBUF,&(b->bufs[i].buffer))<0) {
			perror("VIDIOC_QBUF");
			break;
		}
	}
	return i;
}
int start_stream(int fd) {
	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	return ioctl(fd, VIDIOC_STREAMON, &type);
}
int stop_stream(int fd) {
	int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	return ioctl(fd, VIDIOC_STREAMOFF, &type);
}
void stream_parm(int fd) {
	struct v4l2_streamparm parm;
	memset (&parm, 0, sizeof(parm));
	parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	parm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
	parm.parm.capture.timeperframe.numerator = 1;
	parm.parm.capture.timeperframe.denominator = 10;
	if(ioctl(fd, VIDIOC_S_PARM, &parm)<0) {
		perror("VIDIOC_S_PARM");
	}
	printf("Framerate: %lf (%d/%d)\n", (double)parm.parm.capture.timeperframe.denominator/parm.parm.capture.timeperframe.numerator, parm.parm.capture.timeperframe.denominator, parm.parm.capture.timeperframe.numerator);
}

int main(int argc, char** argv) {
	int fd = open("/dev/video0", O_RDWR);
	if(fd < 0) {
		perror("open");
		return 1;
	}

	print_caps(fd);
	print_video_inputs(fd);
	switch_video_input(fd, 0);
	print_video_standards(fd);
	print_controls(fd);
	print_extended_controls(fd);
	print_formats(fd);
	stream_parm(fd);
	struct buffers* bufs = init_buffers(fd, 5);
	if(bufs == NULL) {
		perror("init buffers");
		return 1;
	}
	printf("Buffers: %d\n", bufs->size);
	if(queue_all_buffers(fd, bufs)<bufs->size) {
		perror("queue");
		return 1;
	}
	if(start_stream(fd)<0) {
		perror("start stream");
		return 1;
	}

	struct timeval begin, end;
	for(int i=0;i<100;i++) {
		struct v4l2_buffer buffer;
		memset(&buffer, 0, sizeof(struct v4l2_buffer));
		buffer.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
		buffer.memory = V4L2_MEMORY_MMAP;
		if(ioctl(fd, VIDIOC_DQBUF, &buffer)<0) {
			perror("VIDIOC_DQBUF");
			break;
		}
		printf("%d %d %d.%06d\n", i, buffer.index, buffer.timestamp.tv_sec, buffer.timestamp.tv_usec, buffer.flags);
		if (i==0)  begin = buffer.timestamp;
		if (i==99) end = buffer.timestamp;

		if(ioctl(fd, VIDIOC_QBUF, &buffer)<0) {
			perror("VIDIOC_QBUF");
			break;
		}
	}
	printf("Actual framerate: %lf\n", 100 / (double)(end.tv_sec - begin.tv_sec + 1e-6*(end.tv_usec + begin.tv_usec)));
	stop_stream(fd);
	free_buffers(fd, bufs);

	close(fd);
	return 0;
}