• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 #include <string>
2 
3 #include <sys/types.h>
4 #include <sys/stat.h>
5 #include <fcntl.h>
6 #include <linux/videodev2.h>
7 #include <sys/ioctl.h>
8 #include <unistd.h>
9 #include <system_error>
10 
11 #include <kms++/kms++.h>
12 #include <kms++util/kms++util.h>
13 #include <kms++util/videodevice.h>
14 
15 using namespace std;
16 using namespace kms;
17 
18 /*
19  * V4L2 and DRM differ in their interpretation of YUV420::NV12
20  *
21  * V4L2 NV12 is a Y and UV co-located planes in a single plane buffer.
22  * DRM NV12 is a Y and UV planes presented as dual plane buffer,
23  * which is known as NM12 in V4L2.
24  *
25  * Since here we have hybrid DRM/V4L2 user space helper functions
26  * we need to translate DRM::NV12 to V4L2:NM12 pixel format back
27  * and forth to keep the data view consistent.
28  */
29 
30 /* V4L2 helper funcs */
v4l2_get_formats(int fd,uint32_t buf_type)31 static vector<PixelFormat> v4l2_get_formats(int fd, uint32_t buf_type)
32 {
33 	vector<PixelFormat> v;
34 
35 	v4l2_fmtdesc desc{};
36 	desc.type = buf_type;
37 
38 	while (ioctl(fd, VIDIOC_ENUM_FMT, &desc) == 0) {
39 		if (desc.pixelformat == V4L2_PIX_FMT_NV12M)
40 			v.push_back(PixelFormat::NV12);
41 		else if (desc.pixelformat != V4L2_PIX_FMT_NV12)
42 			v.push_back((PixelFormat)desc.pixelformat);
43 
44 		desc.index++;
45 	}
46 
47 	return v;
48 }
49 
v4l2_set_format(int fd,PixelFormat fmt,uint32_t width,uint32_t height,uint32_t buf_type)50 static void v4l2_set_format(int fd, PixelFormat fmt, uint32_t width, uint32_t height, uint32_t buf_type)
51 {
52 	int r;
53 
54 	v4l2_format v4lfmt{};
55 
56 	v4lfmt.type = buf_type;
57 	r = ioctl(fd, VIDIOC_G_FMT, &v4lfmt);
58 	ASSERT(r == 0);
59 
60 	const PixelFormatInfo& pfi = get_pixel_format_info(fmt);
61 
62 	bool mplane = buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE || buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
63 
64 	if (mplane) {
65 		v4l2_pix_format_mplane& mp = v4lfmt.fmt.pix_mp;
66 		uint32_t used_fmt;
67 
68 		if (fmt == PixelFormat::NV12)
69 			used_fmt = V4L2_PIX_FMT_NV12M;
70 		else
71 			used_fmt = (uint32_t)fmt;
72 
73 		mp.pixelformat = used_fmt;
74 		mp.width = width;
75 		mp.height = height;
76 
77 		mp.num_planes = pfi.num_planes;
78 
79 		for (unsigned i = 0; i < pfi.num_planes; ++i) {
80 			const PixelFormatPlaneInfo& pfpi = pfi.planes[i];
81 			v4l2_plane_pix_format& p = mp.plane_fmt[i];
82 
83 			p.bytesperline = width * pfpi.bitspp / 8;
84 			p.sizeimage = p.bytesperline * height / pfpi.ysub;
85 		}
86 
87 		r = ioctl(fd, VIDIOC_S_FMT, &v4lfmt);
88 		ASSERT(r == 0);
89 
90 		ASSERT(mp.pixelformat == used_fmt);
91 		ASSERT(mp.width == width);
92 		ASSERT(mp.height == height);
93 
94 		ASSERT(mp.num_planes == pfi.num_planes);
95 
96 		for (unsigned i = 0; i < pfi.num_planes; ++i) {
97 			const PixelFormatPlaneInfo& pfpi = pfi.planes[i];
98 			v4l2_plane_pix_format& p = mp.plane_fmt[i];
99 
100 			ASSERT(p.bytesperline == width * pfpi.bitspp / 8);
101 			ASSERT(p.sizeimage == p.bytesperline * height / pfpi.ysub);
102 		}
103 	} else {
104 		ASSERT(pfi.num_planes == 1);
105 
106 		v4lfmt.fmt.pix.pixelformat = (uint32_t)fmt;
107 		v4lfmt.fmt.pix.width = width;
108 		v4lfmt.fmt.pix.height = height;
109 		v4lfmt.fmt.pix.bytesperline = width * pfi.planes[0].bitspp / 8;
110 
111 		r = ioctl(fd, VIDIOC_S_FMT, &v4lfmt);
112 		ASSERT(r == 0);
113 
114 		ASSERT(v4lfmt.fmt.pix.pixelformat == (uint32_t)fmt);
115 		ASSERT(v4lfmt.fmt.pix.width == width);
116 		ASSERT(v4lfmt.fmt.pix.height == height);
117 		ASSERT(v4lfmt.fmt.pix.bytesperline == width * pfi.planes[0].bitspp / 8);
118 	}
119 }
120 
v4l2_get_selection(int fd,uint32_t & left,uint32_t & top,uint32_t & width,uint32_t & height,uint32_t buf_type)121 static void v4l2_get_selection(int fd, uint32_t& left, uint32_t& top, uint32_t& width, uint32_t& height, uint32_t buf_type)
122 {
123 	int r;
124 	struct v4l2_selection selection;
125 
126 	if (buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT ||
127 	    buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
128 		selection.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
129 		selection.target = V4L2_SEL_TGT_CROP;
130 	} else if (buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE ||
131 		   buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
132 		selection.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
133 		selection.target = V4L2_SEL_TGT_COMPOSE;
134 	} else {
135 		FAIL("buf_type (%d) is not valid\n", buf_type);
136 	}
137 
138 	r = ioctl(fd, VIDIOC_G_SELECTION, &selection);
139 	ASSERT(r == 0);
140 
141 	left = selection.r.left;
142 	top = selection.r.top;
143 	width = selection.r.width;
144 	height = selection.r.height;
145 }
146 
v4l2_set_selection(int fd,uint32_t & left,uint32_t & top,uint32_t & width,uint32_t & height,uint32_t buf_type)147 static void v4l2_set_selection(int fd, uint32_t& left, uint32_t& top, uint32_t& width, uint32_t& height, uint32_t buf_type)
148 {
149 	int r;
150 	struct v4l2_selection selection;
151 
152 	if (buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT ||
153 	    buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE) {
154 		selection.type = V4L2_BUF_TYPE_VIDEO_OUTPUT;
155 		selection.target = V4L2_SEL_TGT_CROP;
156 	} else if (buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE ||
157 		   buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE) {
158 		selection.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
159 		selection.target = V4L2_SEL_TGT_COMPOSE;
160 	} else {
161 		FAIL("buf_type (%d) is not valid\n", buf_type);
162 	}
163 
164 	selection.r.left = left;
165 	selection.r.top = top;
166 	selection.r.width = width;
167 	selection.r.height = height;
168 
169 	r = ioctl(fd, VIDIOC_S_SELECTION, &selection);
170 	ASSERT(r == 0);
171 
172 	left = selection.r.left;
173 	top = selection.r.top;
174 	width = selection.r.width;
175 	height = selection.r.height;
176 }
177 
v4l2_request_bufs(int fd,uint32_t queue_size,uint32_t buf_type)178 static void v4l2_request_bufs(int fd, uint32_t queue_size, uint32_t buf_type)
179 {
180 	v4l2_requestbuffers v4lreqbuf{};
181 	v4lreqbuf.type = buf_type;
182 	v4lreqbuf.memory = V4L2_MEMORY_DMABUF;
183 	v4lreqbuf.count = queue_size;
184 	int r = ioctl(fd, VIDIOC_REQBUFS, &v4lreqbuf);
185 	ASSERT(r == 0);
186 	ASSERT(v4lreqbuf.count == queue_size);
187 }
188 
v4l2_queue_dmabuf(int fd,uint32_t index,DumbFramebuffer * fb,uint32_t buf_type)189 static void v4l2_queue_dmabuf(int fd, uint32_t index, DumbFramebuffer* fb, uint32_t buf_type)
190 {
191 	v4l2_buffer buf{};
192 	buf.type = buf_type;
193 	buf.memory = V4L2_MEMORY_DMABUF;
194 	buf.index = index;
195 
196 	const PixelFormatInfo& pfi = get_pixel_format_info(fb->format());
197 
198 	bool mplane = buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE || buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
199 
200 	if (mplane) {
201 		buf.length = pfi.num_planes;
202 
203 		v4l2_plane planes[4]{};
204 		buf.m.planes = planes;
205 
206 		for (unsigned i = 0; i < pfi.num_planes; ++i) {
207 			planes[i].m.fd = fb->prime_fd(i);
208 			planes[i].bytesused = fb->size(i);
209 			planes[i].length = fb->size(i);
210 		}
211 
212 		int r = ioctl(fd, VIDIOC_QBUF, &buf);
213 		ASSERT(r == 0);
214 	} else {
215 		buf.m.fd = fb->prime_fd(0);
216 
217 		int r = ioctl(fd, VIDIOC_QBUF, &buf);
218 		ASSERT(r == 0);
219 	}
220 }
221 
v4l2_dequeue(int fd,uint32_t buf_type)222 static uint32_t v4l2_dequeue(int fd, uint32_t buf_type)
223 {
224 	v4l2_buffer buf{};
225 	buf.type = buf_type;
226 	buf.memory = V4L2_MEMORY_DMABUF;
227 
228 	// V4L2 crashes if planes are not set
229 	v4l2_plane planes[4]{};
230 	buf.m.planes = planes;
231 	buf.length = 4;
232 
233 	int r = ioctl(fd, VIDIOC_DQBUF, &buf);
234 	if (r)
235 		throw system_error(errno, generic_category());
236 
237 	return buf.index;
238 }
239 
VideoDevice(const string & dev)240 VideoDevice::VideoDevice(const string& dev)
241 	: VideoDevice(::open(dev.c_str(), O_RDWR | O_NONBLOCK))
242 {
243 }
244 
VideoDevice(int fd)245 VideoDevice::VideoDevice(int fd)
246 	: m_fd(fd), m_has_capture(false), m_has_output(false), m_has_m2m(false)
247 {
248 	if (fd < 0)
249 		throw runtime_error("bad fd");
250 
251 	struct v4l2_capability cap = {};
252 	int r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
253 	ASSERT(r == 0);
254 
255 	if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
256 		m_has_capture = true;
257 		m_has_mplane_capture = true;
258 	} else if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) {
259 		m_has_capture = true;
260 		m_has_mplane_capture = false;
261 	}
262 
263 	if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT_MPLANE) {
264 		m_has_output = true;
265 		m_has_mplane_output = true;
266 	} else if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) {
267 		m_has_output = true;
268 		m_has_mplane_output = false;
269 	}
270 
271 	if (cap.capabilities & V4L2_CAP_VIDEO_M2M_MPLANE) {
272 		m_has_m2m = true;
273 		m_has_capture = true;
274 		m_has_output = true;
275 		m_has_mplane_m2m = true;
276 		m_has_mplane_capture = true;
277 		m_has_mplane_output = true;
278 	} else if (cap.capabilities & V4L2_CAP_VIDEO_M2M) {
279 		m_has_m2m = true;
280 		m_has_capture = true;
281 		m_has_output = true;
282 		m_has_mplane_m2m = false;
283 		m_has_mplane_capture = false;
284 		m_has_mplane_output = false;
285 	}
286 }
287 
~VideoDevice()288 VideoDevice::~VideoDevice()
289 {
290 	::close(m_fd);
291 }
292 
get_capture_streamer()293 VideoStreamer* VideoDevice::get_capture_streamer()
294 {
295 	ASSERT(m_has_capture);
296 
297 	if (!m_capture_streamer) {
298 		auto type = m_has_mplane_capture ? VideoStreamer::StreamerType::CaptureMulti : VideoStreamer::StreamerType::CaptureSingle;
299 		m_capture_streamer = std::unique_ptr<VideoStreamer>(new VideoStreamer(m_fd, type));
300 	}
301 
302 	return m_capture_streamer.get();
303 }
304 
get_output_streamer()305 VideoStreamer* VideoDevice::get_output_streamer()
306 {
307 	ASSERT(m_has_output);
308 
309 	if (!m_output_streamer) {
310 		auto type = m_has_mplane_output ? VideoStreamer::StreamerType::OutputMulti : VideoStreamer::StreamerType::OutputSingle;
311 		m_output_streamer = std::unique_ptr<VideoStreamer>(new VideoStreamer(m_fd, type));
312 	}
313 
314 	return m_output_streamer.get();
315 }
316 
get_discrete_frame_sizes(PixelFormat fmt)317 vector<tuple<uint32_t, uint32_t>> VideoDevice::get_discrete_frame_sizes(PixelFormat fmt)
318 {
319 	vector<tuple<uint32_t, uint32_t>> v;
320 
321 	v4l2_frmsizeenum v4lfrms{};
322 	v4lfrms.pixel_format = (uint32_t)fmt;
323 
324 	int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
325 	ASSERT(r);
326 
327 	FAIL_IF(v4lfrms.type != V4L2_FRMSIZE_TYPE_DISCRETE, "No discrete frame sizes");
328 
329 	while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
330 		v.emplace_back(v4lfrms.discrete.width, v4lfrms.discrete.height);
331 		v4lfrms.index++;
332 	};
333 
334 	return v;
335 }
336 
get_frame_sizes(PixelFormat fmt)337 VideoDevice::VideoFrameSize VideoDevice::get_frame_sizes(PixelFormat fmt)
338 {
339 	v4l2_frmsizeenum v4lfrms{};
340 	v4lfrms.pixel_format = (uint32_t)fmt;
341 
342 	int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
343 	ASSERT(r);
344 
345 	FAIL_IF(v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE, "No continuous frame sizes");
346 
347 	VideoFrameSize s;
348 
349 	s.min_w = v4lfrms.stepwise.min_width;
350 	s.max_w = v4lfrms.stepwise.max_width;
351 	s.step_w = v4lfrms.stepwise.step_width;
352 
353 	s.min_h = v4lfrms.stepwise.min_height;
354 	s.max_h = v4lfrms.stepwise.max_height;
355 	s.step_h = v4lfrms.stepwise.step_height;
356 
357 	return s;
358 }
359 
get_capture_devices()360 vector<string> VideoDevice::get_capture_devices()
361 {
362 	vector<string> v;
363 
364 	for (int i = 0; i < 20; ++i) {
365 		string name = "/dev/video" + to_string(i);
366 
367 		struct stat buffer;
368 		if (stat(name.c_str(), &buffer) != 0)
369 			continue;
370 
371 		try {
372 			VideoDevice vid(name);
373 
374 			if (vid.has_capture() && !vid.has_m2m())
375 				v.push_back(name);
376 		} catch (...) {
377 		}
378 	}
379 
380 	return v;
381 }
382 
get_m2m_devices()383 vector<string> VideoDevice::get_m2m_devices()
384 {
385 	vector<string> v;
386 
387 	for (int i = 0; i < 20; ++i) {
388 		string name = "/dev/video" + to_string(i);
389 
390 		struct stat buffer;
391 		if (stat(name.c_str(), &buffer) != 0)
392 			continue;
393 
394 		try {
395 			VideoDevice vid(name);
396 
397 			if (vid.has_m2m())
398 				v.push_back(name);
399 		} catch (...) {
400 		}
401 	}
402 
403 	return v;
404 }
405 
VideoStreamer(int fd,StreamerType type)406 VideoStreamer::VideoStreamer(int fd, StreamerType type)
407 	: m_fd(fd), m_type(type)
408 {
409 }
410 
get_ports()411 std::vector<string> VideoStreamer::get_ports()
412 {
413 	vector<string> v;
414 
415 	switch (m_type) {
416 	case StreamerType::CaptureSingle:
417 	case StreamerType::CaptureMulti: {
418 		struct v4l2_input input {
419 		};
420 
421 		while (ioctl(m_fd, VIDIOC_ENUMINPUT, &input) == 0) {
422 			v.push_back(string((char*)&input.name));
423 			input.index++;
424 		}
425 
426 		break;
427 	}
428 
429 	case StreamerType::OutputSingle:
430 	case StreamerType::OutputMulti: {
431 		struct v4l2_output output {
432 		};
433 
434 		while (ioctl(m_fd, VIDIOC_ENUMOUTPUT, &output) == 0) {
435 			v.push_back(string((char*)&output.name));
436 			output.index++;
437 		}
438 
439 		break;
440 	}
441 
442 	default:
443 		FAIL("Bad StreamerType");
444 	}
445 
446 	return v;
447 }
448 
set_port(uint32_t index)449 void VideoStreamer::set_port(uint32_t index)
450 {
451 	unsigned long req;
452 
453 	switch (m_type) {
454 	case StreamerType::CaptureSingle:
455 	case StreamerType::CaptureMulti:
456 		req = VIDIOC_S_INPUT;
457 		break;
458 
459 	case StreamerType::OutputSingle:
460 	case StreamerType::OutputMulti:
461 		req = VIDIOC_S_OUTPUT;
462 		break;
463 
464 	default:
465 		FAIL("Bad StreamerType");
466 	}
467 
468 	int r = ioctl(m_fd, req, &index);
469 	ASSERT(r == 0);
470 }
471 
get_buf_type(VideoStreamer::StreamerType type)472 static v4l2_buf_type get_buf_type(VideoStreamer::StreamerType type)
473 {
474 	switch (type) {
475 	case VideoStreamer::StreamerType::CaptureSingle:
476 		return V4L2_BUF_TYPE_VIDEO_CAPTURE;
477 	case VideoStreamer::StreamerType::CaptureMulti:
478 		return V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
479 	case VideoStreamer::StreamerType::OutputSingle:
480 		return V4L2_BUF_TYPE_VIDEO_OUTPUT;
481 	case VideoStreamer::StreamerType::OutputMulti:
482 		return V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
483 	default:
484 		FAIL("Bad StreamerType");
485 	}
486 }
487 
get_formats()488 std::vector<PixelFormat> VideoStreamer::get_formats()
489 {
490 	return v4l2_get_formats(m_fd, get_buf_type(m_type));
491 }
492 
set_format(PixelFormat fmt,uint32_t width,uint32_t height)493 void VideoStreamer::set_format(PixelFormat fmt, uint32_t width, uint32_t height)
494 {
495 	v4l2_set_format(m_fd, fmt, width, height, get_buf_type(m_type));
496 }
497 
get_selection(uint32_t & left,uint32_t & top,uint32_t & width,uint32_t & height)498 void VideoStreamer::get_selection(uint32_t& left, uint32_t& top, uint32_t& width, uint32_t& height)
499 {
500 	v4l2_get_selection(m_fd, left, top, width, height, get_buf_type(m_type));
501 }
502 
set_selection(uint32_t & left,uint32_t & top,uint32_t & width,uint32_t & height)503 void VideoStreamer::set_selection(uint32_t& left, uint32_t& top, uint32_t& width, uint32_t& height)
504 {
505 	v4l2_set_selection(m_fd, left, top, width, height, get_buf_type(m_type));
506 }
507 
set_queue_size(uint32_t queue_size)508 void VideoStreamer::set_queue_size(uint32_t queue_size)
509 {
510 	v4l2_request_bufs(m_fd, queue_size, get_buf_type(m_type));
511 	m_fbs.resize(queue_size);
512 }
513 
queue(DumbFramebuffer * fb)514 void VideoStreamer::queue(DumbFramebuffer* fb)
515 {
516 	uint32_t idx;
517 
518 	for (idx = 0; idx < m_fbs.size(); ++idx) {
519 		if (m_fbs[idx] == nullptr)
520 			break;
521 	}
522 
523 	FAIL_IF(idx == m_fbs.size(), "queue full");
524 
525 	m_fbs[idx] = fb;
526 
527 	v4l2_queue_dmabuf(m_fd, idx, fb, get_buf_type(m_type));
528 }
529 
dequeue()530 DumbFramebuffer* VideoStreamer::dequeue()
531 {
532 	uint32_t idx = v4l2_dequeue(m_fd, get_buf_type(m_type));
533 
534 	auto fb = m_fbs[idx];
535 	m_fbs[idx] = nullptr;
536 
537 	return fb;
538 }
539 
stream_on()540 void VideoStreamer::stream_on()
541 {
542 	uint32_t buf_type = get_buf_type(m_type);
543 	int r = ioctl(m_fd, VIDIOC_STREAMON, &buf_type);
544 	FAIL_IF(r, "Failed to enable stream: %d", r);
545 }
546 
stream_off()547 void VideoStreamer::stream_off()
548 {
549 	uint32_t buf_type = get_buf_type(m_type);
550 	int r = ioctl(m_fd, VIDIOC_STREAMOFF, &buf_type);
551 	FAIL_IF(r, "Failed to disable stream: %d", r);
552 }
553