tests: kmscapture: simple v4l2 capture test tool

Simple tool on top of libkms to display video from attached V4L cameras.
w/o parameter the kmscapture will try to use all cameras. With -s/--single
 cmd line parameter it can be forced to open only /dev/video0.
The camera resolution is chosen based on the screen resolution, number of
cameras and the supported resolutions by the camera itself.
By default the buffer is provided by DRM. This can be changed with the
--buffer-type=v4l, so the buffer is provided by the V4L layer.

2 files changed, 392 insertions, 0 deletions

diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 181ff78..bb335d3 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -15,3 +15,6 @@ target_link_libraries(kmsprint kms++ kmstest ${LIBDRM_LIBRARIES})
 
 add_executable (fbtestpat fbtestpat.cpp)
 target_link_libraries(fbtestpat kmstest)
+
+add_executable (kmscapture kmscapture.cpp)
+target_link_libraries(kmscapture kms++ kmstest ${LIBDRM_LIBRARIES})
diff --git a/tests/kmscapture.cpp b/tests/kmscapture.cpp
new file mode 100644
index 0000000..d433c44
--- /dev/null
+++ b/tests/kmscapture.cpp
@@ -0,0 +1,389 @@
+#include <linux/videodev2.h>
+#include <cstdio>
+#include <string.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <fstream>
+#include <sys/ioctl.h>
+#include <xf86drm.h>
+
+#include "kms++.h"
+#include "test.h"
+#include "opts.h"
+
+#define CAMERA_BUF_QUEUE_SIZE	3
+#define MAX_CAMERA		9
+
+using namespace std;
+using namespace kms;
+
+enum buffer_provider {
+	BUFFER_DRM = 0,
+	BUFFER_V4L,
+};
+
+class Camera
+{
+public:
+	Camera(int camera_id, Card& card, Plane* plane, uint32_t x, uint32_t y,
+	       uint32_t iw, uint32_t ih, PixelFormat pixfmt,
+	       enum buffer_provider buffer_type);
+	~Camera();
+
+	Camera(const Camera& other) = delete;
+	Camera& operator=(const Camera& other) = delete;
+
+	void show_next_frame(Crtc* crtc);
+	int fd() const { return m_fd; }
+private:
+	ExtFramebuffer* GetExtFrameBuffer(Card& card, int i, PixelFormat pixfmt);
+	int m_fd;	/* camera file descriptor */
+	Plane* m_plane;
+	enum buffer_provider m_buffer_type;
+	vector<DumbFramebuffer*> m_fb; /* framebuffers for DRM buffers */
+	vector<ExtFramebuffer*> m_extfb; /* framebuffers for V4L2 buffers */
+	int m_prev_fb_index;
+	uint32_t m_in_width, m_in_height; /* camera capture resolution */
+	/* image properties for display */
+	uint32_t m_out_width, m_out_height;
+	uint32_t m_out_x, m_out_y;
+};
+
+static int buffer_export(int v4lfd, enum v4l2_buf_type bt, int index, int *dmafd)
+{
+	struct v4l2_exportbuffer expbuf;
+
+	memset(&expbuf, 0, sizeof(expbuf));
+	expbuf.type = bt;
+	expbuf.index = index;
+	if (ioctl(v4lfd, VIDIOC_EXPBUF, &expbuf) == -1) {
+		perror("VIDIOC_EXPBUF");
+		return -1;
+	}
+
+	*dmafd = expbuf.fd;
+
+	return 0;
+}
+
+ExtFramebuffer* Camera::GetExtFrameBuffer(Card& card, int i, PixelFormat pixfmt)
+{
+	int r, dmafd;
+
+	r = buffer_export(m_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, i, &dmafd);
+	ASSERT(r == 0);
+
+	uint32_t handle;
+	r = drmPrimeFDToHandle(card.fd(), dmafd, &handle);
+	ASSERT(r == 0);
+
+	const PixelFormatInfo& format_info = get_pixel_format_info(pixfmt);
+	ASSERT(format_info.num_planes == 1);
+
+	uint32_t handles[4] { handle };
+	uint32_t pitches[4] { m_in_width * (format_info.planes[0].bitspp / 8) };
+	uint32_t offsets[4] { };
+
+	return new ExtFramebuffer(card, m_in_width, m_in_height, pixfmt,
+				  handles, pitches, offsets);
+}
+
+bool inline better_size(struct v4l2_frmsize_discrete* v4ldisc,
+			uint32_t iw, uint32_t ih,
+			uint32_t best_w, uint32_t best_h)
+{
+	if (v4ldisc->width <= iw && v4ldisc->height <= ih &&
+	    (v4ldisc->width >= best_w || v4ldisc->height >= best_h))
+		return true;
+
+	return false;
+}
+
+Camera::Camera(int camera_id, Card& card, Plane* plane, uint32_t x, uint32_t y,
+	       uint32_t iw, uint32_t ih, PixelFormat pixfmt,
+	       enum buffer_provider buffer_type)
+{
+	char dev_name[20];
+	int r, i;
+	uint32_t best_w = 320;
+	uint32_t best_h = 240;
+	uint32_t v4l_mem;
+
+	m_buffer_type = buffer_type;
+	if (m_buffer_type == BUFFER_V4L)
+		v4l_mem = V4L2_MEMORY_MMAP;
+	else
+		v4l_mem = V4L2_MEMORY_DMABUF;
+
+	sprintf(dev_name, "/dev/video%d", camera_id);
+	m_fd = ::open(dev_name, O_RDWR | O_NONBLOCK);
+
+	ASSERT(m_fd >= 0);
+
+	struct v4l2_frmsizeenum v4lfrms = { 0 };
+	v4lfrms.pixel_format = (uint32_t) pixfmt;
+	while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
+		if (v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
+			if (better_size(&v4lfrms.discrete, iw, ih,
+					best_w, best_h)) {
+				best_w = v4lfrms.discrete.width;
+				best_h = v4lfrms.discrete.height;
+			}
+		} else {
+			break;
+		}
+		v4lfrms.index++;
+	};
+
+	m_out_width = m_in_width = best_w;
+	m_out_height = m_in_height = best_h;
+	/* Move it to the middle of the requested area */
+	m_out_x = x + iw / 2 - m_out_width / 2;
+	m_out_y = y + ih / 2 - m_out_height / 2;
+
+	struct v4l2_format v4lfmt = { 0 };
+	v4lfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	r = ioctl(m_fd, VIDIOC_G_FMT, &v4lfmt);
+	ASSERT(r == 0);
+
+	v4lfmt.fmt.pix.pixelformat = (uint32_t) pixfmt;
+	v4lfmt.fmt.pix.width = m_in_width;
+	v4lfmt.fmt.pix.height = m_in_height;
+
+	r = ioctl(m_fd, VIDIOC_S_FMT, &v4lfmt);
+	ASSERT(r == 0);
+
+	struct v4l2_requestbuffers v4lreqbuf = { 0 };
+	v4lreqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	v4lreqbuf.memory = v4l_mem;
+	v4lreqbuf.count = CAMERA_BUF_QUEUE_SIZE;
+	r = ioctl(m_fd, VIDIOC_REQBUFS, &v4lreqbuf);
+	ASSERT(r == 0);
+	ASSERT(v4lreqbuf.count == CAMERA_BUF_QUEUE_SIZE);
+
+	struct v4l2_buffer v4lbuf = { 0 };
+	v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	v4lbuf.memory = v4l_mem;
+
+	for (i = 0; i < CAMERA_BUF_QUEUE_SIZE; i++) {
+		DumbFramebuffer *fb = NULL;
+		ExtFramebuffer *extfb = NULL;
+
+		if (m_buffer_type == BUFFER_V4L)
+			extfb = GetExtFrameBuffer(card, i, pixfmt);
+		else
+			fb = new DumbFramebuffer(card, m_in_width,
+						 m_in_height, pixfmt);
+
+		v4lbuf.index = i;
+		if (m_buffer_type == BUFFER_DRM)
+			v4lbuf.m.fd = fb->prime_fd(0);
+		r = ioctl(m_fd, VIDIOC_QBUF, &v4lbuf);
+		ASSERT(r == 0);
+
+		if (m_buffer_type == BUFFER_V4L)
+			m_extfb.push_back(extfb);
+		else
+			m_fb.push_back(fb);
+	}
+	m_prev_fb_index = -1;
+	enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+	r = ioctl(m_fd, VIDIOC_STREAMON, &type);
+		ASSERT(r == 0);
+
+	m_plane = plane;
+}
+
+
+Camera::~Camera()
+{
+	for (unsigned i = 0; i < m_fb.size(); i++)
+		delete m_fb[i];
+
+	for (unsigned i = 0; i < m_extfb.size(); i++)
+		delete m_extfb[i];
+
+	::close(m_fd);
+}
+
+void Camera::show_next_frame(Crtc* crtc)
+{
+	int r;
+	int fb_index;
+	uint32_t v4l_mem;
+
+	if (m_buffer_type == BUFFER_V4L)
+		v4l_mem = V4L2_MEMORY_MMAP;
+	else
+		v4l_mem = V4L2_MEMORY_DMABUF;
+
+	struct v4l2_buffer v4l2buf = { 0 };
+	v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+	v4l2buf.memory = v4l_mem;
+	r = ioctl(m_fd, VIDIOC_DQBUF, &v4l2buf);
+	if (r != 0) {
+		printf("VIDIOC_DQBUF ioctl failed with %d\n", errno);
+		return;
+	}
+
+	fb_index = v4l2buf.index;
+	if (m_buffer_type == BUFFER_V4L)
+		r = crtc->set_plane(m_plane, *m_extfb[fb_index],
+				    m_out_x, m_out_y, m_out_width, m_out_height,
+				    0, 0, m_in_width, m_in_height);
+	else
+		r = crtc->set_plane(m_plane, *m_fb[fb_index],
+				    m_out_x, m_out_y, m_out_width, m_out_height,
+				    0, 0, m_in_width, m_in_height);
+
+	ASSERT(r == 0);
+
+	if (m_prev_fb_index >= 0) {
+		memset(&v4l2buf, 0, sizeof(v4l2buf));
+		v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+		v4l2buf.memory = v4l_mem;
+		v4l2buf.index = m_prev_fb_index;
+		if (m_buffer_type == BUFFER_DRM)
+			v4l2buf.m.fd = m_fb[m_prev_fb_index]->prime_fd(0);
+		r = ioctl(m_fd, VIDIOC_QBUF, &v4l2buf);
+		ASSERT(r == 0);
+
+	}
+	m_prev_fb_index = fb_index;
+}
+
+
+static bool is_capture_dev(int fd)
+{
+	struct v4l2_capability cap = { 0 };
+	int r;
+
+	r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
+	ASSERT(r == 0);
+	if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
+		return false;
+
+	return true;
+}
+
+static vector<int> count_cameras()
+{
+	int i, fd;
+	vector<int> camera_idx;
+	char dev_name[20];
+
+	for (i = 0; i < MAX_CAMERA; i++) {
+		sprintf(dev_name, "/dev/video%d", i);
+		fd = ::open(dev_name, O_RDWR | O_NONBLOCK);
+		if (fd >= 0) {
+			if (is_capture_dev(fd))
+				camera_idx.push_back(i);
+			close(fd);
+		}
+	}
+
+	return camera_idx;
+}
+
+static const char* usage_str =
+"Usage: kmscapture [OPTIONS]\n\n"
+"Options:\n"
+"  -s, --single                Single camera mode. Open only /dev/video0\n"
+"      --buffer-type=<drm|v4l> Use DRM or V4L provided buffers. Default: DRM\n"
+"  -h, --help                  Print this help\n"
+;
+
+int main(int argc, char** argv)
+{
+	uint32_t w;
+	enum buffer_provider buffer_type = BUFFER_DRM;
+	int i;
+
+	auto camera_idx = count_cameras();
+	int nr_cameras = camera_idx.size();
+
+	FAIL_IF(!nr_cameras, "Not a single camera has been found.");
+
+	OptionSet optionset = {
+		Option("s|single", [&](string s)
+		{
+			nr_cameras = 1;
+		}),
+		Option("|buffer-type=", [&](string s)
+		{
+			if (!s.compare("v4l"))
+				buffer_type = BUFFER_V4L;
+			else if (s.compare("drm"))
+				printf("invalid buffer-type: %s\n", s.c_str());
+		}),
+		Option("h|help", [&]()
+		{
+			puts(usage_str);
+			exit(-1);
+		}),
+	};
+
+	optionset.parse(argc, argv);
+
+	if (optionset.params().size() > 0) {
+		puts(usage_str);
+		exit(-1);
+	}
+
+	auto pixfmt = FourCCToPixelFormat("YUYV");
+
+	Card card;
+
+	auto conn = card.get_first_connected_connector();
+	auto crtc = conn->get_current_crtc();
+	printf("Display: %dx%d\n", crtc->width(), crtc->height());
+	printf("Buffer provider: %s\n",
+	       buffer_type == BUFFER_V4L? "V4L" : "DRM");
+
+	w = crtc->width() / nr_cameras;
+	vector<Camera*> cameras;
+	i = 0;
+	for (Plane* p : crtc->get_possible_planes()) {
+		if (p->plane_type() != PlaneType::Overlay)
+			continue;
+
+		if (!p->supports_format(pixfmt))
+			continue;
+
+		auto cam = new Camera(camera_idx[i], card, p, i * w, 0,
+				      w, crtc->height(), pixfmt, buffer_type);
+		cameras.push_back(cam);
+		if (++i == nr_cameras)
+			break;
+	}
+
+	FAIL_IF(i < nr_cameras, "available plane not found");
+
+	struct pollfd fds[nr_cameras + 1] = { 0 };
+	for (i = 0; i < nr_cameras; i++) {
+		fds[i].fd = cameras[i]->fd();
+		fds[i].events =  POLLIN;
+	}
+	fds[nr_cameras].fd = 0;
+	fds[nr_cameras].events =  POLLIN;
+
+	while (true) {
+		int r = poll(fds, nr_cameras + 1, -1);
+		ASSERT(r > 0);
+
+		if (fds[nr_cameras].revents != 0)
+			break;
+
+		for (i = 0; i < nr_cameras; i++) {
+			if (!fds[i].revents)
+				continue;
+			cameras[i]->show_next_frame(crtc);
+			fds[i].revents = 0;
+		}
+	}
+
+	for (auto cam : cameras)
+		delete cam;
+}