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