kmscapture: fix v4l buftype
[android/external-libkmsxx.git] / utils / kmscapture.cpp
1 #include <linux/videodev2.h>
2 #include <cstdio>
3 #include <string.h>
4 #include <poll.h>
5 #include <fcntl.h>
6 #include <unistd.h>
7 #include <fstream>
8 #include <sys/ioctl.h>
9 #include <xf86drm.h>
10 #include <glob.h>
12 #include <kms++/kms++.h>
13 #include <kms++util/kms++util.h>
15 #define CAMERA_BUF_QUEUE_SIZE   3
16 #define MAX_CAMERA              9
18 using namespace std;
19 using namespace kms;
21 enum class BufferProvider {
22         DRM,
23         V4L2,
24 };
26 class CameraPipeline
27 {
28 public:
29         CameraPipeline(int cam_fd, Card& card, Crtc* crtc, Plane* plane, uint32_t x, uint32_t y,
30                        uint32_t iw, uint32_t ih, PixelFormat pixfmt,
31                        BufferProvider buffer_provider);
32         ~CameraPipeline();
34         CameraPipeline(const CameraPipeline& other) = delete;
35         CameraPipeline& operator=(const CameraPipeline& other) = delete;
37         void show_next_frame(AtomicReq &req);
38         int fd() const { return m_fd; }
39         void start_streaming();
40 private:
41         ExtFramebuffer* GetExtFrameBuffer(Card& card, uint32_t i, PixelFormat pixfmt);
42         int m_fd;       /* camera file descriptor */
43         Crtc* m_crtc;
44         Plane* m_plane;
45         BufferProvider m_buffer_provider;
46         vector<DumbFramebuffer*> m_fb; /* framebuffers for DRM buffers */
47         vector<ExtFramebuffer*> m_extfb; /* framebuffers for V4L2 buffers */
48         int m_prev_fb_index;
49         uint32_t m_in_width, m_in_height; /* camera capture resolution */
50         /* image properties for display */
51         uint32_t m_out_width, m_out_height;
52         uint32_t m_out_x, m_out_y;
53 };
55 static int buffer_export(int v4lfd, enum v4l2_buf_type bt, uint32_t index, int *dmafd)
56 {
57         struct v4l2_exportbuffer expbuf;
59         memset(&expbuf, 0, sizeof(expbuf));
60         expbuf.type = bt;
61         expbuf.index = index;
62         if (ioctl(v4lfd, VIDIOC_EXPBUF, &expbuf) == -1) {
63                 perror("VIDIOC_EXPBUF");
64                 return -1;
65         }
67         *dmafd = expbuf.fd;
69         return 0;
70 }
72 ExtFramebuffer* CameraPipeline::GetExtFrameBuffer(Card& card, uint32_t i, PixelFormat pixfmt)
73 {
74         int r, dmafd;
76         r = buffer_export(m_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, i, &dmafd);
77         ASSERT(r == 0);
79         const PixelFormatInfo& format_info = get_pixel_format_info(pixfmt);
80         ASSERT(format_info.num_planes == 1);
82         vector<int> fds { dmafd };
83         vector<uint32_t> pitches { m_in_width * (format_info.planes[0].bitspp / 8) };
84         vector<uint32_t> offsets { 0 };
86         return new ExtFramebuffer(card, m_in_width, m_in_height, pixfmt,
87                                   fds, pitches, offsets);
88 }
90 bool inline better_size(struct v4l2_frmsize_discrete* v4ldisc,
91                         uint32_t iw, uint32_t ih,
92                         uint32_t best_w, uint32_t best_h)
93 {
94         if (v4ldisc->width <= iw && v4ldisc->height <= ih &&
95             (v4ldisc->width >= best_w || v4ldisc->height >= best_h))
96                 return true;
98         return false;
99 }
101 CameraPipeline::CameraPipeline(int cam_fd, Card& card, Crtc *crtc, Plane* plane, uint32_t x, uint32_t y,
102                                uint32_t iw, uint32_t ih, PixelFormat pixfmt,
103                                BufferProvider buffer_provider)
104         : m_fd(cam_fd), m_crtc(crtc), m_buffer_provider(buffer_provider), m_prev_fb_index(-1)
107         int r;
108         uint32_t best_w = 320;
109         uint32_t best_h = 240;
111         struct v4l2_frmsizeenum v4lfrms = { };
112         v4lfrms.pixel_format = (uint32_t)pixfmt;
113         while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
114                 if (v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
115                         if (better_size(&v4lfrms.discrete, iw, ih,
116                                         best_w, best_h)) {
117                                 best_w = v4lfrms.discrete.width;
118                                 best_h = v4lfrms.discrete.height;
119                         }
120                 } else {
121                         break;
122                 }
123                 v4lfrms.index++;
124         };
126         m_out_width = m_in_width = best_w;
127         m_out_height = m_in_height = best_h;
128         /* Move it to the middle of the requested area */
129         m_out_x = x + iw / 2 - m_out_width / 2;
130         m_out_y = y + ih / 2 - m_out_height / 2;
132         struct v4l2_format v4lfmt = { };
133         v4lfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
134         r = ioctl(m_fd, VIDIOC_G_FMT, &v4lfmt);
135         ASSERT(r == 0);
137         v4lfmt.fmt.pix.pixelformat = (uint32_t)pixfmt;
138         v4lfmt.fmt.pix.width = m_in_width;
139         v4lfmt.fmt.pix.height = m_in_height;
141         r = ioctl(m_fd, VIDIOC_S_FMT, &v4lfmt);
142         ASSERT(r == 0);
144         uint32_t v4l_mem;
146         if (m_buffer_provider == BufferProvider::V4L2)
147                 v4l_mem = V4L2_MEMORY_MMAP;
148         else
149                 v4l_mem = V4L2_MEMORY_DMABUF;
151         struct v4l2_requestbuffers v4lreqbuf = { };
152         v4lreqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
153         v4lreqbuf.memory = v4l_mem;
154         v4lreqbuf.count = CAMERA_BUF_QUEUE_SIZE;
155         r = ioctl(m_fd, VIDIOC_REQBUFS, &v4lreqbuf);
156         ASSERT(r == 0);
157         ASSERT(v4lreqbuf.count == CAMERA_BUF_QUEUE_SIZE);
159         struct v4l2_buffer v4lbuf = { };
160         v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
161         v4lbuf.memory = v4l_mem;
163         for (unsigned i = 0; i < CAMERA_BUF_QUEUE_SIZE; i++) {
164                 DumbFramebuffer *fb = NULL;
165                 ExtFramebuffer *extfb = NULL;
167                 if (m_buffer_provider == BufferProvider::V4L2)
168                         extfb = GetExtFrameBuffer(card, i, pixfmt);
169                 else
170                         fb = new DumbFramebuffer(card, m_in_width,
171                                                  m_in_height, pixfmt);
173                 v4lbuf.index = i;
174                 if (m_buffer_provider == BufferProvider::DRM)
175                         v4lbuf.m.fd = fb->prime_fd(0);
176                 r = ioctl(m_fd, VIDIOC_QBUF, &v4lbuf);
177                 ASSERT(r == 0);
179                 if (m_buffer_provider == BufferProvider::V4L2)
180                         m_extfb.push_back(extfb);
181                 else
182                         m_fb.push_back(fb);
183         }
185         m_plane = plane;
187         // Do initial plane setup with first fb, so that we only need to
188         // set the FB when page flipping
189         AtomicReq req(card);
191         Framebuffer *fb;
192         if (m_buffer_provider == BufferProvider::V4L2)
193                 fb = m_extfb[0];
194         else
195                 fb = m_fb[0];
197         req.add(m_plane, "CRTC_ID", m_crtc->id());
198         req.add(m_plane, "FB_ID", fb->id());
200         req.add(m_plane, "CRTC_X", m_out_x);
201         req.add(m_plane, "CRTC_Y", m_out_y);
202         req.add(m_plane, "CRTC_W", m_out_width);
203         req.add(m_plane, "CRTC_H", m_out_height);
205         req.add(m_plane, "SRC_X", 0);
206         req.add(m_plane, "SRC_Y", 0);
207         req.add(m_plane, "SRC_W", m_in_width << 16);
208         req.add(m_plane, "SRC_H", m_in_height << 16);
210         r = req.commit_sync();
211         FAIL_IF(r, "initial plane setup failed");
214 CameraPipeline::~CameraPipeline()
216         for (unsigned i = 0; i < m_fb.size(); i++)
217                 delete m_fb[i];
219         for (unsigned i = 0; i < m_extfb.size(); i++)
220                 delete m_extfb[i];
222         ::close(m_fd);
225 void CameraPipeline::start_streaming()
227         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
229         int r = ioctl(m_fd, VIDIOC_STREAMON, &type);
230         FAIL_IF(r, "Failed to enable camera stream: %d", r);
233 void CameraPipeline::show_next_frame(AtomicReq& req)
235         int r;
236         uint32_t v4l_mem;
238         if (m_buffer_provider == BufferProvider::V4L2)
239                 v4l_mem = V4L2_MEMORY_MMAP;
240         else
241                 v4l_mem = V4L2_MEMORY_DMABUF;
243         struct v4l2_buffer v4l2buf = { };
244         v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
245         v4l2buf.memory = v4l_mem;
246         r = ioctl(m_fd, VIDIOC_DQBUF, &v4l2buf);
247         if (r != 0) {
248                 printf("VIDIOC_DQBUF ioctl failed with %d\n", errno);
249                 return;
250         }
252         unsigned fb_index = v4l2buf.index;
254         Framebuffer *fb;
255         if (m_buffer_provider == BufferProvider::V4L2)
256                 fb = m_extfb[fb_index];
257         else
258                 fb = m_fb[fb_index];
260         req.add(m_plane, "FB_ID", fb->id());
262         if (m_prev_fb_index >= 0) {
263                 memset(&v4l2buf, 0, sizeof(v4l2buf));
264                 v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
265                 v4l2buf.memory = v4l_mem;
266                 v4l2buf.index = m_prev_fb_index;
267                 if (m_buffer_provider == BufferProvider::DRM)
268                         v4l2buf.m.fd = m_fb[m_prev_fb_index]->prime_fd(0);
269                 r = ioctl(m_fd, VIDIOC_QBUF, &v4l2buf);
270                 ASSERT(r == 0);
272         }
274         m_prev_fb_index = fb_index;
277 static bool is_capture_dev(int fd)
279         struct v4l2_capability cap = { };
280         int r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
281         ASSERT(r == 0);
282         return cap.capabilities & V4L2_CAP_VIDEO_CAPTURE;
285 std::vector<std::string> glob(const std::string& pat)
287         glob_t glob_result;
288         glob(pat.c_str(), 0, NULL, &glob_result);
289         vector<string> ret;
290         for(unsigned i = 0; i < glob_result.gl_pathc; ++i)
291                 ret.push_back(string(glob_result.gl_pathv[i]));
292         globfree(&glob_result);
293         return ret;
296 static const char* usage_str =
297                 "Usage: kmscapture [OPTIONS]\n\n"
298                 "Options:\n"
299                 "  -s, --single                Single camera mode. Open only /dev/video0\n"
300                 "      --buffer-type=<drm|v4l> Use DRM or V4L provided buffers. Default: DRM\n"
301                 "  -h, --help                  Print this help\n"
302                 ;
304 int main(int argc, char** argv)
306         BufferProvider buffer_provider = BufferProvider::DRM;
307         bool single_cam = false;
309         OptionSet optionset = {
310                 Option("s|single", [&]()
311                 {
312                         single_cam = true;
313                 }),
314                 Option("|buffer-type=", [&](string s)
315                 {
316                         if (s == "v4l")
317                                 buffer_provider = BufferProvider::V4L2;
318                         else if (s == "drm")
319                                 buffer_provider = BufferProvider::DRM;
320                         else
321                                 FAIL("Invalid buffer provider: %s", s.c_str());
322                 }),
323                 Option("h|help", [&]()
324                 {
325                         puts(usage_str);
326                         exit(-1);
327                 }),
328         };
330         optionset.parse(argc, argv);
332         if (optionset.params().size() > 0) {
333                 puts(usage_str);
334                 exit(-1);
335         }
337         auto pixfmt = PixelFormat::YUYV;
339         Card card;
341         auto conn = card.get_first_connected_connector();
342         auto crtc = conn->get_current_crtc();
343         printf("Display: %dx%d\n", crtc->width(), crtc->height());
344         printf("Buffer provider: %s\n", buffer_provider == BufferProvider::V4L2? "V4L" : "DRM");
346         vector<int> camera_fds;
348         for (string vidpath : glob("/dev/video*")) {
349                 int fd = ::open(vidpath.c_str(), O_RDWR | O_NONBLOCK);
351                 if (fd < 0)
352                         continue;
354                 if (!is_capture_dev(fd)) {
355                         close(fd);
356                         continue;
357                 }
359                 camera_fds.push_back(fd);
361                 if (single_cam)
362                         break;
363         }
365         FAIL_IF(camera_fds.size() == 0, "No cameras found");
367         vector<Plane*> available_planes;
368         for (Plane* p : crtc->get_possible_planes()) {
369                 if (p->plane_type() != PlaneType::Overlay)
370                         continue;
372                 if (!p->supports_format(pixfmt))
373                         continue;
375                 available_planes.push_back(p);
376         }
378         FAIL_IF(available_planes.size() < camera_fds.size(), "Not enough video planes for cameras");
380         uint32_t plane_w = crtc->width() / camera_fds.size();
381         vector<CameraPipeline*> cameras;
383         for (unsigned i = 0; i < camera_fds.size(); ++i) {
384                 int cam_fd = camera_fds[i];
385                 Plane* plane = available_planes[i];
387                 auto cam = new CameraPipeline(cam_fd, card, crtc, plane, i * plane_w, 0,
388                                               plane_w, crtc->height(), pixfmt, buffer_provider);
389                 cameras.push_back(cam);
390         }
392         unsigned nr_cameras = cameras.size();
394         vector<pollfd> fds(nr_cameras + 1);
396         for (unsigned i = 0; i < nr_cameras; i++) {
397                 fds[i].fd = cameras[i]->fd();
398                 fds[i].events =  POLLIN;
399         }
400         fds[nr_cameras].fd = 0;
401         fds[nr_cameras].events =  POLLIN;
403         for (auto cam : cameras)
404                 cam->start_streaming();
406         while (true) {
407                 int r = poll(fds.data(), nr_cameras + 1, -1);
408                 ASSERT(r > 0);
410                 if (fds[nr_cameras].revents != 0)
411                         break;
413                 AtomicReq req(card);
415                 for (unsigned i = 0; i < nr_cameras; i++) {
416                         if (!fds[i].revents)
417                                 continue;
418                         cameras[i]->show_next_frame(req);
419                         fds[i].revents = 0;
420                 }
422                 r = req.test();
423                 FAIL_IF(r, "Atomic commit failed: %d", r);
425                 req.commit_sync();
426         }
428         for (auto cam : cameras)
429                 delete cam;