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