Merge V4L2 related work
[android/external-libkmsxx.git] / kms++util / src / videodevice.cpp
1 #include <string>
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>
10 #include <kms++/kms++.h>
11 #include <kms++util/kms++util.h>
12 #include <kms++util/videodevice.h>
14 using namespace std;
15 using namespace kms;
17 /* V4L2 helper funcs */
18 static vector<PixelFormat> v4l2_get_formats(int fd, uint32_t buf_type)
19 {
20         vector<PixelFormat> v;
22         v4l2_fmtdesc desc { };
23         desc.type = buf_type;
25         while (ioctl(fd, VIDIOC_ENUM_FMT, &desc) == 0) {
26                 v.push_back((PixelFormat)desc.pixelformat);
27                 desc.index++;
28         }
30         return v;
31 }
33 static void v4l2_set_format(int fd, PixelFormat fmt, uint32_t width, uint32_t height, uint32_t buf_type)
34 {
35         int r;
37         v4l2_format v4lfmt { };
39         v4lfmt.type = buf_type;
40         r = ioctl(fd, VIDIOC_G_FMT, &v4lfmt);
41         ASSERT(r == 0);
43         const PixelFormatInfo& pfi = get_pixel_format_info(fmt);
45         bool mplane = buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE || buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
47         if (mplane) {
48                 v4l2_pix_format_mplane& mp = v4lfmt.fmt.pix_mp;
50                 mp.pixelformat = (uint32_t)fmt;
51                 mp.width = width;
52                 mp.height = height;
54                 mp.num_planes = pfi.num_planes;
56                 for (unsigned i = 0; i < pfi.num_planes; ++i) {
57                         const PixelFormatPlaneInfo& pfpi = pfi.planes[i];
58                         v4l2_plane_pix_format& p = mp.plane_fmt[i];
60                         p.bytesperline = width * pfpi.bitspp / 8;
61                         p.sizeimage = p.bytesperline * height / pfpi.ysub;
62                 }
64                 r = ioctl(fd, VIDIOC_S_FMT, &v4lfmt);
65                 ASSERT(r == 0);
67                 ASSERT(mp.pixelformat == (uint32_t)fmt);
68                 ASSERT(mp.width == width);
69                 ASSERT(mp.height == height);
71                 ASSERT(mp.num_planes == pfi.num_planes);
73                 for (unsigned i = 0; i < pfi.num_planes; ++i) {
74                         const PixelFormatPlaneInfo& pfpi = pfi.planes[i];
75                         v4l2_plane_pix_format& p = mp.plane_fmt[i];
77                         ASSERT(p.bytesperline == width * pfpi.bitspp / 8);
78                         ASSERT(p.sizeimage == p.bytesperline * height / pfpi.ysub);
79                 }
80         } else {
81                 ASSERT(pfi.num_planes == 1);
83                 v4lfmt.fmt.pix.pixelformat = (uint32_t)fmt;
84                 v4lfmt.fmt.pix.width = width;
85                 v4lfmt.fmt.pix.height = height;
86                 v4lfmt.fmt.pix.bytesperline = width * pfi.planes[0].bitspp / 8;
88                 r = ioctl(fd, VIDIOC_S_FMT, &v4lfmt);
89                 ASSERT(r == 0);
91                 ASSERT(v4lfmt.fmt.pix.pixelformat == (uint32_t)fmt);
92                 ASSERT(v4lfmt.fmt.pix.width == width);
93                 ASSERT(v4lfmt.fmt.pix.height == height);
94                 ASSERT(v4lfmt.fmt.pix.bytesperline == width * pfi.planes[0].bitspp / 8);
95         }
96 }
98 static void v4l2_request_bufs(int fd, uint32_t queue_size, uint32_t buf_type)
99 {
100         v4l2_requestbuffers v4lreqbuf { };
101         v4lreqbuf.type = buf_type;
102         v4lreqbuf.memory = V4L2_MEMORY_DMABUF;
103         v4lreqbuf.count = queue_size;
104         int r = ioctl(fd, VIDIOC_REQBUFS, &v4lreqbuf);
105         ASSERT(r == 0);
106         ASSERT(v4lreqbuf.count == queue_size);
109 static void v4l2_queue_dmabuf(int fd, uint32_t index, DumbFramebuffer* fb, uint32_t buf_type)
111         v4l2_buffer buf { };
112         buf.type = buf_type;
113         buf.memory = V4L2_MEMORY_DMABUF;
114         buf.index = index;
116         const PixelFormatInfo& pfi = get_pixel_format_info(fb->format());
118         bool mplane = buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE || buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
120         if (mplane) {
121                 buf.length = pfi.num_planes;
123                 v4l2_plane planes[4] { };
124                 buf.m.planes = planes;
126                 for (unsigned i = 0; i < pfi.num_planes; ++i) {
127                         planes[i].m.fd = fb->prime_fd(i);
128                         planes[i].bytesused = fb->size(i);
129                         planes[i].length = fb->size(i);
130                 }
132                 int r = ioctl(fd, VIDIOC_QBUF, &buf);
133                 ASSERT(r == 0);
134         } else {
135                 buf.m.fd = fb->prime_fd(0);
137                 int r = ioctl(fd, VIDIOC_QBUF, &buf);
138                 ASSERT(r == 0);
139         }
142 static uint32_t v4l2_dequeue(int fd, uint32_t buf_type)
144         v4l2_buffer buf { };
145         buf.type = buf_type;
146         buf.memory = V4L2_MEMORY_DMABUF;
148         // V4L2 crashes if planes are not set
149         v4l2_plane planes[4] { };
150         buf.m.planes = planes;
151         buf.length = 4;
153         int r = ioctl(fd, VIDIOC_DQBUF, &buf);
154         if (r)
155                 throw system_error(errno, generic_category());
157         return buf.index;
163 VideoDevice::VideoDevice(const string& dev)
164         :VideoDevice(::open(dev.c_str(), O_RDWR | O_NONBLOCK))
168 VideoDevice::VideoDevice(int fd)
169         : m_fd(fd), m_has_capture(false), m_has_output(false), m_has_m2m(false), m_capture_streamer(0), m_output_streamer(0)
171         FAIL_IF(fd < 0, "Bad fd");
173         struct v4l2_capability cap = { };
174         int r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
175         ASSERT(r == 0);
177         if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE_MPLANE) {
178                 m_has_capture = true;
179                 m_has_mplane_capture = true;
180         } else if (cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) {
181                 m_has_capture = true;
182                 m_has_mplane_capture = false;
183         }
185         if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT_MPLANE) {
186                 m_has_output = true;
187                 m_has_mplane_output = true;
188         } else if (cap.capabilities & V4L2_CAP_VIDEO_OUTPUT) {
189                 m_has_output = true;
190                 m_has_mplane_output = false;
191         }
193         if (cap.capabilities & V4L2_CAP_VIDEO_M2M_MPLANE) {
194                 m_has_m2m = true;
195                 m_has_capture = true;
196                 m_has_output = true;
197                 m_has_mplane_m2m = true;
198                 m_has_mplane_capture = true;
199                 m_has_mplane_output = true;
200         } else if (cap.capabilities & V4L2_CAP_VIDEO_M2M) {
201                 m_has_m2m = true;
202                 m_has_capture = true;
203                 m_has_output = true;
204                 m_has_mplane_m2m = false;
205                 m_has_mplane_capture = false;
206                 m_has_mplane_output = false;
207         }
210 VideoDevice::~VideoDevice()
212         ::close(m_fd);
215 VideoStreamer* VideoDevice::get_capture_streamer()
217         ASSERT(m_has_capture);
219         if (!m_capture_streamer) {
220                 auto type = m_has_mplane_capture ? VideoStreamer::StreamerType::CaptureMulti : VideoStreamer::StreamerType::CaptureSingle;
221                 m_capture_streamer = new VideoStreamer(m_fd, type);
222         }
224         return m_capture_streamer;
227 VideoStreamer* VideoDevice::get_output_streamer()
229         ASSERT(m_has_output);
231         if (!m_output_streamer) {
232                 auto type = m_has_mplane_output ? VideoStreamer::StreamerType::OutputMulti : VideoStreamer::StreamerType::OutputSingle;
233                 m_output_streamer = new VideoStreamer(m_fd, type);
234         }
236         return m_output_streamer;
239 vector<tuple<uint32_t, uint32_t>> VideoDevice::get_discrete_frame_sizes(PixelFormat fmt)
241         vector<tuple<uint32_t, uint32_t>> v;
243         v4l2_frmsizeenum v4lfrms { };
244         v4lfrms.pixel_format = (uint32_t)fmt;
246         int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
247         ASSERT(r);
249         FAIL_IF(v4lfrms.type != V4L2_FRMSIZE_TYPE_DISCRETE, "No discrete frame sizes");
251         while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
252                 v.emplace_back(v4lfrms.discrete.width, v4lfrms.discrete.height);
253                 v4lfrms.index++;
254         };
256         return v;
259 VideoDevice::VideoFrameSize VideoDevice::get_frame_sizes(PixelFormat fmt)
261         v4l2_frmsizeenum v4lfrms { };
262         v4lfrms.pixel_format = (uint32_t)fmt;
264         int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
265         ASSERT(r);
267         FAIL_IF(v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE, "No continuous frame sizes");
269         VideoFrameSize s;
271         s.min_w = v4lfrms.stepwise.min_width;
272         s.max_w = v4lfrms.stepwise.max_width;
273         s.step_w = v4lfrms.stepwise.step_width;
275         s.min_h = v4lfrms.stepwise.min_height;
276         s.max_h = v4lfrms.stepwise.max_height;
277         s.step_h = v4lfrms.stepwise.step_height;
279         return s;
282 vector<string> VideoDevice::get_capture_devices()
284         vector<string> v;
286         for (int i = 0; i < 20; ++i) {
287                 string name = "/dev/video" + to_string(i);
289                 struct stat buffer;
290                 if (stat(name.c_str(), &buffer) != 0)
291                         continue;
293                 VideoDevice vid(name);
295                 if (vid.has_capture() && !vid.has_m2m())
296                         v.push_back(name);
297         }
299         return v;
302 vector<string> VideoDevice::get_m2m_devices()
304         vector<string> v;
306         for (int i = 0; i < 20; ++i) {
307                 string name = "/dev/video" + to_string(i);
309                 struct stat buffer;
310                 if (stat(name.c_str(), &buffer) != 0)
311                         continue;
313                 VideoDevice vid(name);
315                 if (vid.has_m2m())
316                         v.push_back(name);
317         }
319         return v;
323 VideoStreamer::VideoStreamer(int fd, StreamerType type)
324         : m_fd(fd), m_type(type)
329 std::vector<string> VideoStreamer::get_ports()
331         vector<string> v;
333         switch (m_type) {
334         case StreamerType::CaptureSingle:
335         case StreamerType::CaptureMulti:
336         {
337                 struct v4l2_input input { };
339                 while (ioctl(m_fd, VIDIOC_ENUMINPUT, &input) == 0) {
340                         v.push_back(string((char*)&input.name));
341                         input.index++;
342                 }
344                 break;
345         }
347         case StreamerType::OutputSingle:
348         case StreamerType::OutputMulti:
349         {
350                 struct v4l2_output output { };
352                 while (ioctl(m_fd, VIDIOC_ENUMOUTPUT, &output) == 0) {
353                         v.push_back(string((char*)&output.name));
354                         output.index++;
355                 }
357                 break;
358         }
360         default:
361                 FAIL("Bad StreamerType");
362         }
364         return v;
367 void VideoStreamer::set_port(uint32_t index)
369         unsigned long req;
371         switch (m_type) {
372         case StreamerType::CaptureSingle:
373         case StreamerType::CaptureMulti:
374                 req = VIDIOC_S_INPUT;
375                 break;
377         case StreamerType::OutputSingle:
378         case StreamerType::OutputMulti:
379                 req = VIDIOC_S_OUTPUT;
380                 break;
382         default:
383                 FAIL("Bad StreamerType");
384         }
386         int r = ioctl(m_fd, req, &index);
387         ASSERT(r == 0);
390 static v4l2_buf_type get_buf_type(VideoStreamer::StreamerType type)
392         switch (type) {
393         case VideoStreamer::StreamerType::CaptureSingle:
394                 return V4L2_BUF_TYPE_VIDEO_CAPTURE;
395         case VideoStreamer::StreamerType::CaptureMulti:
396                 return V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
397         case VideoStreamer::StreamerType::OutputSingle:
398                 return V4L2_BUF_TYPE_VIDEO_OUTPUT;
399         case VideoStreamer::StreamerType::OutputMulti:
400                 return V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
401         default:
402                 FAIL("Bad StreamerType");
403         }
406 std::vector<PixelFormat> VideoStreamer::get_formats()
408         return v4l2_get_formats(m_fd, get_buf_type(m_type));
411 void VideoStreamer::set_format(PixelFormat fmt, uint32_t width, uint32_t height)
413         v4l2_set_format(m_fd, fmt, width, height, get_buf_type(m_type));
416 void VideoStreamer::set_queue_size(uint32_t queue_size)
418         v4l2_request_bufs(m_fd, queue_size, get_buf_type(m_type));
419         m_fbs.resize(queue_size);
422 void VideoStreamer::queue(DumbFramebuffer* fb)
424         uint32_t idx;
426         for (idx = 0; idx < m_fbs.size(); ++idx) {
427                 if (m_fbs[idx] == nullptr)
428                         break;
429         }
431         FAIL_IF(idx == m_fbs.size(), "queue full");
433         m_fbs[idx] = fb;
435         v4l2_queue_dmabuf(m_fd, idx, fb, get_buf_type(m_type));
438 DumbFramebuffer*VideoStreamer::dequeue()
440         uint32_t idx = v4l2_dequeue(m_fd, get_buf_type(m_type));
442         auto fb = m_fbs[idx];
443         m_fbs[idx] = nullptr;
445         return fb;
448 void VideoStreamer::stream_on()
450         uint32_t buf_type = get_buf_type(m_type);
451         int r = ioctl(m_fd, VIDIOC_STREAMON, &buf_type);
452         FAIL_IF(r, "Failed to enable stream: %d", r);
455 void VideoStreamer::stream_off()
457         uint32_t buf_type = get_buf_type(m_type);
458         int r = ioctl(m_fd, VIDIOC_STREAMOFF, &buf_type);
459         FAIL_IF(r, "Failed to disable stream: %d", r);