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_request_bufs(int fd,uint32_t queue_size,uint32_t buf_type)99 static void v4l2_request_bufs(int fd, uint32_t queue_size, uint32_t buf_type)
100 {
101 v4l2_requestbuffers v4lreqbuf { };
102 v4lreqbuf.type = buf_type;
103 v4lreqbuf.memory = V4L2_MEMORY_DMABUF;
104 v4lreqbuf.count = queue_size;
105 int r = ioctl(fd, VIDIOC_REQBUFS, &v4lreqbuf);
106 ASSERT(r == 0);
107 ASSERT(v4lreqbuf.count == queue_size);
108 }
109
v4l2_queue_dmabuf(int fd,uint32_t index,DumbFramebuffer * fb,uint32_t buf_type)110 static void v4l2_queue_dmabuf(int fd, uint32_t index, DumbFramebuffer* fb, uint32_t buf_type)
111 {
112 v4l2_buffer buf { };
113 buf.type = buf_type;
114 buf.memory = V4L2_MEMORY_DMABUF;
115 buf.index = index;
116
117 const PixelFormatInfo& pfi = get_pixel_format_info(fb->format());
118
119 bool mplane = buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE || buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
120
121 if (mplane) {
122 buf.length = pfi.num_planes;
123
124 v4l2_plane planes[4] { };
125 buf.m.planes = planes;
126
127 for (unsigned i = 0; i < pfi.num_planes; ++i) {
128 planes[i].m.fd = fb->prime_fd(i);
129 planes[i].bytesused = fb->size(i);
130 planes[i].length = fb->size(i);
131 }
132
133 int r = ioctl(fd, VIDIOC_QBUF, &buf);
134 ASSERT(r == 0);
135 } else {
136 buf.m.fd = fb->prime_fd(0);
137
138 int r = ioctl(fd, VIDIOC_QBUF, &buf);
139 ASSERT(r == 0);
140 }
141 }
142
v4l2_dequeue(int fd,uint32_t buf_type)143 static uint32_t v4l2_dequeue(int fd, uint32_t buf_type)
144 {
145 v4l2_buffer buf { };
146 buf.type = buf_type;
147 buf.memory = V4L2_MEMORY_DMABUF;
148
149 // V4L2 crashes if planes are not set
150 v4l2_plane planes[4] { };
151 buf.m.planes = planes;
152 buf.length = 4;
153
154 int r = ioctl(fd, VIDIOC_DQBUF, &buf);
155 if (r)
156 throw system_error(errno, generic_category());
157
158 return buf.index;
159 }
160
161
162
163
VideoDevice(const string & dev)164 VideoDevice::VideoDevice(const string& dev)
165 :VideoDevice(::open(dev.c_str(), O_RDWR | O_NONBLOCK))
166 {
167 }
168
VideoDevice(int fd)169 VideoDevice::VideoDevice(int fd)
170 : m_fd(fd), m_has_capture(false), m_has_output(false), m_has_m2m(false), m_capture_streamer(0), m_output_streamer(0)
171 {
172 FAIL_IF(fd < 0, "Bad fd");
173
174 struct v4l2_capability cap = { };
175 int r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
176 ASSERT(r == 0);
177
178 if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
179 m_has_capture = true;
180 m_has_mplane_capture = true;
181 } else if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) {
182 m_has_capture = true;
183 m_has_mplane_capture = false;
184 }
185
186 if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT_MPLANE) {
187 m_has_output = true;
188 m_has_mplane_output = true;
189 } else if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) {
190 m_has_output = true;
191 m_has_mplane_output = false;
192 }
193
194 if (cap.capabilities & V4L2_CAP_VIDEO_M2M_MPLANE) {
195 m_has_m2m = true;
196 m_has_capture = true;
197 m_has_output = true;
198 m_has_mplane_m2m = true;
199 m_has_mplane_capture = true;
200 m_has_mplane_output = true;
201 } else if (cap.capabilities & V4L2_CAP_VIDEO_M2M) {
202 m_has_m2m = true;
203 m_has_capture = true;
204 m_has_output = true;
205 m_has_mplane_m2m = false;
206 m_has_mplane_capture = false;
207 m_has_mplane_output = false;
208 }
209 }
210
~VideoDevice()211 VideoDevice::~VideoDevice()
212 {
213 ::close(m_fd);
214 }
215
get_capture_streamer()216 VideoStreamer* VideoDevice::get_capture_streamer()
217 {
218 ASSERT(m_has_capture);
219
220 if (!m_capture_streamer) {
221 auto type = m_has_mplane_capture ? VideoStreamer::StreamerType::CaptureMulti : VideoStreamer::StreamerType::CaptureSingle;
222 m_capture_streamer = new VideoStreamer(m_fd, type);
223 }
224
225 return m_capture_streamer;
226 }
227
get_output_streamer()228 VideoStreamer* VideoDevice::get_output_streamer()
229 {
230 ASSERT(m_has_output);
231
232 if (!m_output_streamer) {
233 auto type = m_has_mplane_output ? VideoStreamer::StreamerType::OutputMulti : VideoStreamer::StreamerType::OutputSingle;
234 m_output_streamer = new VideoStreamer(m_fd, type);
235 }
236
237 return m_output_streamer;
238 }
239
get_discrete_frame_sizes(PixelFormat fmt)240 vector<tuple<uint32_t, uint32_t>> VideoDevice::get_discrete_frame_sizes(PixelFormat fmt)
241 {
242 vector<tuple<uint32_t, uint32_t>> v;
243
244 v4l2_frmsizeenum v4lfrms { };
245 v4lfrms.pixel_format = (uint32_t)fmt;
246
247 int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
248 ASSERT(r);
249
250 FAIL_IF(v4lfrms.type != V4L2_FRMSIZE_TYPE_DISCRETE, "No discrete frame sizes");
251
252 while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
253 v.emplace_back(v4lfrms.discrete.width, v4lfrms.discrete.height);
254 v4lfrms.index++;
255 };
256
257 return v;
258 }
259
get_frame_sizes(PixelFormat fmt)260 VideoDevice::VideoFrameSize VideoDevice::get_frame_sizes(PixelFormat fmt)
261 {
262 v4l2_frmsizeenum v4lfrms { };
263 v4lfrms.pixel_format = (uint32_t)fmt;
264
265 int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
266 ASSERT(r);
267
268 FAIL_IF(v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE, "No continuous frame sizes");
269
270 VideoFrameSize s;
271
272 s.min_w = v4lfrms.stepwise.min_width;
273 s.max_w = v4lfrms.stepwise.max_width;
274 s.step_w = v4lfrms.stepwise.step_width;
275
276 s.min_h = v4lfrms.stepwise.min_height;
277 s.max_h = v4lfrms.stepwise.max_height;
278 s.step_h = v4lfrms.stepwise.step_height;
279
280 return s;
281 }
282
get_capture_devices()283 vector<string> VideoDevice::get_capture_devices()
284 {
285 vector<string> v;
286
287 for (int i = 0; i < 20; ++i) {
288 string name = "/dev/video" + to_string(i);
289
290 struct stat buffer;
291 if (stat(name.c_str(), &buffer) != 0)
292 continue;
293
294 VideoDevice vid(name);
295
296 if (vid.has_capture() && !vid.has_m2m())
297 v.push_back(name);
298 }
299
300 return v;
301 }
302
get_m2m_devices()303 vector<string> VideoDevice::get_m2m_devices()
304 {
305 vector<string> v;
306
307 for (int i = 0; i < 20; ++i) {
308 string name = "/dev/video" + to_string(i);
309
310 struct stat buffer;
311 if (stat(name.c_str(), &buffer) != 0)
312 continue;
313
314 VideoDevice vid(name);
315
316 if (vid.has_m2m())
317 v.push_back(name);
318 }
319
320 return v;
321 }
322
323
VideoStreamer(int fd,StreamerType type)324 VideoStreamer::VideoStreamer(int fd, StreamerType type)
325 : m_fd(fd), m_type(type)
326 {
327
328 }
329
get_ports()330 std::vector<string> VideoStreamer::get_ports()
331 {
332 vector<string> v;
333
334 switch (m_type) {
335 case StreamerType::CaptureSingle:
336 case StreamerType::CaptureMulti:
337 {
338 struct v4l2_input input { };
339
340 while (ioctl(m_fd, VIDIOC_ENUMINPUT, &input) == 0) {
341 v.push_back(string((char*)&input.name));
342 input.index++;
343 }
344
345 break;
346 }
347
348 case StreamerType::OutputSingle:
349 case StreamerType::OutputMulti:
350 {
351 struct v4l2_output output { };
352
353 while (ioctl(m_fd, VIDIOC_ENUMOUTPUT, &output) == 0) {
354 v.push_back(string((char*)&output.name));
355 output.index++;
356 }
357
358 break;
359 }
360
361 default:
362 FAIL("Bad StreamerType");
363 }
364
365 return v;
366 }
367
set_port(uint32_t index)368 void VideoStreamer::set_port(uint32_t index)
369 {
370 unsigned long req;
371
372 switch (m_type) {
373 case StreamerType::CaptureSingle:
374 case StreamerType::CaptureMulti:
375 req = VIDIOC_S_INPUT;
376 break;
377
378 case StreamerType::OutputSingle:
379 case StreamerType::OutputMulti:
380 req = VIDIOC_S_OUTPUT;
381 break;
382
383 default:
384 FAIL("Bad StreamerType");
385 }
386
387 int r = ioctl(m_fd, req, &index);
388 ASSERT(r == 0);
389 }
390
get_buf_type(VideoStreamer::StreamerType type)391 static v4l2_buf_type get_buf_type(VideoStreamer::StreamerType type)
392 {
393 switch (type) {
394 case VideoStreamer::StreamerType::CaptureSingle:
395 return V4L2_BUF_TYPE_VIDEO_CAPTURE;
396 case VideoStreamer::StreamerType::CaptureMulti:
397 return V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
398 case VideoStreamer::StreamerType::OutputSingle:
399 return V4L2_BUF_TYPE_VIDEO_OUTPUT;
400 case VideoStreamer::StreamerType::OutputMulti:
401 return V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
402 default:
403 FAIL("Bad StreamerType");
404 }
405 }
406
get_formats()407 std::vector<PixelFormat> VideoStreamer::get_formats()
408 {
409 return v4l2_get_formats(m_fd, get_buf_type(m_type));
410 }
411
set_format(PixelFormat fmt,uint32_t width,uint32_t height)412 void VideoStreamer::set_format(PixelFormat fmt, uint32_t width, uint32_t height)
413 {
414 v4l2_set_format(m_fd, fmt, width, height, get_buf_type(m_type));
415 }
416
set_queue_size(uint32_t queue_size)417 void VideoStreamer::set_queue_size(uint32_t queue_size)
418 {
419 v4l2_request_bufs(m_fd, queue_size, get_buf_type(m_type));
420 m_fbs.resize(queue_size);
421 }
422
queue(DumbFramebuffer * fb)423 void VideoStreamer::queue(DumbFramebuffer* fb)
424 {
425 uint32_t idx;
426
427 for (idx = 0; idx < m_fbs.size(); ++idx) {
428 if (m_fbs[idx] == nullptr)
429 break;
430 }
431
432 FAIL_IF(idx == m_fbs.size(), "queue full");
433
434 m_fbs[idx] = fb;
435
436 v4l2_queue_dmabuf(m_fd, idx, fb, get_buf_type(m_type));
437 }
438
dequeue()439 DumbFramebuffer* VideoStreamer::dequeue()
440 {
441 uint32_t idx = v4l2_dequeue(m_fd, get_buf_type(m_type));
442
443 auto fb = m_fbs[idx];
444 m_fbs[idx] = nullptr;
445
446 return fb;
447 }
448
stream_on()449 void VideoStreamer::stream_on()
450 {
451 uint32_t buf_type = get_buf_type(m_type);
452 int r = ioctl(m_fd, VIDIOC_STREAMON, &buf_type);
453 FAIL_IF(r, "Failed to enable stream: %d", r);
454 }
455
stream_off()456 void VideoStreamer::stream_off()
457 {
458 uint32_t buf_type = get_buf_type(m_type);
459 int r = ioctl(m_fd, VIDIOC_STREAMOFF, &buf_type);
460 FAIL_IF(r, "Failed to disable stream: %d", r);
461 }
462