kmacapture: atomic modesetting support
[android/external-libkmsxx.git] / tests / 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++.h"
13 #include "test.h"
14 #include "opts.h"
16 #define CAMERA_BUF_QUEUE_SIZE   3
17 #define MAX_CAMERA              9
19 using namespace std;
20 using namespace kms;
22 enum class BufferProvider {
23         DRM,
24         V4L2,
25 };
27 class CameraPipeline
28 {
29 public:
30         CameraPipeline(int cam_fd, Card& card, Crtc* crtc, Plane* plane, uint32_t x, uint32_t y,
31                        uint32_t iw, uint32_t ih, PixelFormat pixfmt,
32                        BufferProvider buffer_provider);
33         ~CameraPipeline();
35         CameraPipeline(const CameraPipeline& other) = delete;
36         CameraPipeline& operator=(const CameraPipeline& other) = delete;
38         void show_next_frame(AtomicReq &req);
39         int fd() const { return m_fd; }
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         uint32_t handle;
80         r = drmPrimeFDToHandle(card.fd(), dmafd, &handle);
81         ASSERT(r == 0);
83         const PixelFormatInfo& format_info = get_pixel_format_info(pixfmt);
84         ASSERT(format_info.num_planes == 1);
86         uint32_t handles[4] { handle };
87         uint32_t pitches[4] { m_in_width * (format_info.planes[0].bitspp / 8) };
88         uint32_t offsets[4] { };
90         return new ExtFramebuffer(card, m_in_width, m_in_height, pixfmt,
91                                   handles, pitches, offsets);
92 }
94 bool inline better_size(struct v4l2_frmsize_discrete* v4ldisc,
95                         uint32_t iw, uint32_t ih,
96                         uint32_t best_w, uint32_t best_h)
97 {
98         if (v4ldisc->width <= iw && v4ldisc->height <= ih &&
99             (v4ldisc->width >= best_w || v4ldisc->height >= best_h))
100                 return true;
102         return false;
105 CameraPipeline::CameraPipeline(int cam_fd, Card& card, Crtc *crtc, Plane* plane, uint32_t x, uint32_t y,
106                                uint32_t iw, uint32_t ih, PixelFormat pixfmt,
107                                BufferProvider buffer_provider)
108         : m_fd(cam_fd), m_crtc(crtc), m_buffer_provider(buffer_provider), m_prev_fb_index(-1)
111         int r;
112         uint32_t best_w = 320;
113         uint32_t best_h = 240;
115         struct v4l2_frmsizeenum v4lfrms = { };
116         v4lfrms.pixel_format = (uint32_t)pixfmt;
117         while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
118                 if (v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
119                         if (better_size(&v4lfrms.discrete, iw, ih,
120                                         best_w, best_h)) {
121                                 best_w = v4lfrms.discrete.width;
122                                 best_h = v4lfrms.discrete.height;
123                         }
124                 } else {
125                         break;
126                 }
127                 v4lfrms.index++;
128         };
130         m_out_width = m_in_width = best_w;
131         m_out_height = m_in_height = best_h;
132         /* Move it to the middle of the requested area */
133         m_out_x = x + iw / 2 - m_out_width / 2;
134         m_out_y = y + ih / 2 - m_out_height / 2;
136         struct v4l2_format v4lfmt = { };
137         v4lfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
138         r = ioctl(m_fd, VIDIOC_G_FMT, &v4lfmt);
139         ASSERT(r == 0);
141         v4lfmt.fmt.pix.pixelformat = (uint32_t)pixfmt;
142         v4lfmt.fmt.pix.width = m_in_width;
143         v4lfmt.fmt.pix.height = m_in_height;
145         r = ioctl(m_fd, VIDIOC_S_FMT, &v4lfmt);
146         ASSERT(r == 0);
148         uint32_t v4l_mem;
150         if (m_buffer_provider == BufferProvider::V4L2)
151                 v4l_mem = V4L2_MEMORY_MMAP;
152         else
153                 v4l_mem = V4L2_MEMORY_DMABUF;
155         struct v4l2_requestbuffers v4lreqbuf = { };
156         v4lreqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
157         v4lreqbuf.memory = v4l_mem;
158         v4lreqbuf.count = CAMERA_BUF_QUEUE_SIZE;
159         r = ioctl(m_fd, VIDIOC_REQBUFS, &v4lreqbuf);
160         ASSERT(r == 0);
161         ASSERT(v4lreqbuf.count == CAMERA_BUF_QUEUE_SIZE);
163         struct v4l2_buffer v4lbuf = { };
164         v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
165         v4lbuf.memory = v4l_mem;
167         for (unsigned i = 0; i < CAMERA_BUF_QUEUE_SIZE; i++) {
168                 DumbFramebuffer *fb = NULL;
169                 ExtFramebuffer *extfb = NULL;
171                 if (m_buffer_provider == BufferProvider::V4L2)
172                         extfb = GetExtFrameBuffer(card, i, pixfmt);
173                 else
174                         fb = new DumbFramebuffer(card, m_in_width,
175                                                  m_in_height, pixfmt);
177                 v4lbuf.index = i;
178                 if (m_buffer_provider == BufferProvider::DRM)
179                         v4lbuf.m.fd = fb->prime_fd(0);
180                 r = ioctl(m_fd, VIDIOC_QBUF, &v4lbuf);
181                 ASSERT(r == 0);
183                 if (m_buffer_provider == BufferProvider::V4L2)
184                         m_extfb.push_back(extfb);
185                 else
186                         m_fb.push_back(fb);
187         }
189         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
191         r = ioctl(m_fd, VIDIOC_STREAMON, &type);
192         ASSERT(r == 0);
194         m_plane = plane;
196         // Do initial plane setup with first fb, so that we only need to
197         // set the FB when page flipping
198         AtomicReq req(card);
200         Framebuffer *fb;
201         if (m_buffer_provider == BufferProvider::V4L2)
202                 fb = m_extfb[0];
203         else
204                 fb = m_fb[0];
206         req.add(m_plane, "CRTC_ID", m_crtc->id());
207         req.add(m_plane, "FB_ID", fb->id());
209         req.add(m_plane, "CRTC_X", m_out_x);
210         req.add(m_plane, "CRTC_Y", m_out_y);
211         req.add(m_plane, "CRTC_W", m_out_width);
212         req.add(m_plane, "CRTC_H", m_out_height);
214         req.add(m_plane, "SRC_X", 0);
215         req.add(m_plane, "SRC_Y", 0);
216         req.add(m_plane, "SRC_W", m_in_width << 16);
217         req.add(m_plane, "SRC_H", m_in_height << 16);
219         r = req.commit_sync();
220         FAIL_IF(r, "initial plane setup failed");
224 CameraPipeline::~CameraPipeline()
226         for (unsigned i = 0; i < m_fb.size(); i++)
227                 delete m_fb[i];
229         for (unsigned i = 0; i < m_extfb.size(); i++)
230                 delete m_extfb[i];
232         ::close(m_fd);
235 void CameraPipeline::show_next_frame(AtomicReq& req)
237         int r;
238         uint32_t v4l_mem;
240         if (m_buffer_provider == BufferProvider::V4L2)
241                 v4l_mem = V4L2_MEMORY_MMAP;
242         else
243                 v4l_mem = V4L2_MEMORY_DMABUF;
245         struct v4l2_buffer v4l2buf = { };
246         v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
247         v4l2buf.memory = v4l_mem;
248         r = ioctl(m_fd, VIDIOC_DQBUF, &v4l2buf);
249         if (r != 0) {
250                 printf("VIDIOC_DQBUF ioctl failed with %d\n", errno);
251                 return;
252         }
254         unsigned fb_index = v4l2buf.index;
256         Framebuffer *fb;
257         if (m_buffer_provider == BufferProvider::V4L2)
258                 fb = m_extfb[fb_index];
259         else
260                 fb = m_fb[fb_index];
262         req.add(m_plane, "FB_ID", fb->id());
264         if (m_prev_fb_index >= 0) {
265                 memset(&v4l2buf, 0, sizeof(v4l2buf));
266                 v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
267                 v4l2buf.memory = v4l_mem;
268                 v4l2buf.index = m_prev_fb_index;
269                 if (m_buffer_provider == BufferProvider::DRM)
270                         v4l2buf.m.fd = m_fb[m_prev_fb_index]->prime_fd(0);
271                 r = ioctl(m_fd, VIDIOC_QBUF, &v4l2buf);
272                 ASSERT(r == 0);
274         }
276         m_prev_fb_index = fb_index;
279 static bool is_capture_dev(int fd)
281         struct v4l2_capability cap = { };
282         int r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
283         ASSERT(r == 0);
284         return cap.capabilities & V4L2_CAP_VIDEO_CAPTURE;
287 std::vector<std::string> glob(const std::string& pat)
289         glob_t glob_result;
290         glob(pat.c_str(), 0, NULL, &glob_result);
291         vector<string> ret;
292         for(unsigned i = 0; i < glob_result.gl_pathc; ++i)
293                 ret.push_back(string(glob_result.gl_pathv[i]));
294         globfree(&glob_result);
295         return ret;
298 static const char* usage_str =
299                 "Usage: kmscapture [OPTIONS]\n\n"
300                 "Options:\n"
301                 "  -s, --single                Single camera mode. Open only /dev/video0\n"
302                 "      --buffer-type=<drm|v4l> Use DRM or V4L provided buffers. Default: DRM\n"
303                 "  -h, --help                  Print this help\n"
304                 ;
306 int main(int argc, char** argv)
308         BufferProvider buffer_provider = BufferProvider::DRM;
309         bool single_cam = false;
311         OptionSet optionset = {
312                 Option("s|single", [&]()
313                 {
314                         single_cam = true;
315                 }),
316                 Option("|buffer-type=", [&](string s)
317                 {
318                         if (s == "v4l")
319                                 buffer_provider = BufferProvider::V4L2;
320                         else if (s == "drm")
321                                 buffer_provider = BufferProvider::DRM;
322                         else
323                                 FAIL("Invalid buffer provider: %s", s.c_str());
324                 }),
325                 Option("h|help", [&]()
326                 {
327                         puts(usage_str);
328                         exit(-1);
329                 }),
330         };
332         optionset.parse(argc, argv);
334         if (optionset.params().size() > 0) {
335                 puts(usage_str);
336                 exit(-1);
337         }
339         auto pixfmt = PixelFormat::YUYV;
341         Card card;
343         auto conn = card.get_first_connected_connector();
344         auto crtc = conn->get_current_crtc();
345         printf("Display: %dx%d\n", crtc->width(), crtc->height());
346         printf("Buffer provider: %s\n", buffer_provider == BufferProvider::V4L2? "V4L" : "DRM");
348         vector<int> camera_fds;
350         for (string vidpath : glob("/dev/video*")) {
351                 int fd = ::open(vidpath.c_str(), O_RDWR | O_NONBLOCK);
353                 if (fd < 0)
354                         continue;
356                 if (!is_capture_dev(fd)) {
357                         close(fd);
358                         continue;
359                 }
361                 camera_fds.push_back(fd);
363                 if (single_cam)
364                         break;
365         }
367         FAIL_IF(camera_fds.size() == 0, "No cameras found");
369         vector<Plane*> available_planes;
370         for (Plane* p : crtc->get_possible_planes()) {
371                 if (p->plane_type() != PlaneType::Overlay)
372                         continue;
374                 if (!p->supports_format(pixfmt))
375                         continue;
377                 available_planes.push_back(p);
378         }
380         FAIL_IF(available_planes.size() < camera_fds.size(), "Not enough video planes for cameras");
382         uint32_t plane_w = crtc->width() / camera_fds.size();
383         vector<CameraPipeline*> cameras;
385         for (unsigned i = 0; i < camera_fds.size(); ++i) {
386                 int cam_fd = camera_fds[i];
387                 Plane* plane = available_planes[i];
389                 auto cam = new CameraPipeline(cam_fd, card, crtc, plane, i * plane_w, 0,
390                                               plane_w, crtc->height(), pixfmt, buffer_provider);
391                 cameras.push_back(cam);
392         }
394         unsigned nr_cameras = cameras.size();
396         vector<pollfd> fds(nr_cameras + 1);
398         for (unsigned i = 0; i < nr_cameras; i++) {
399                 fds[i].fd = cameras[i]->fd();
400                 fds[i].events =  POLLIN;
401         }
402         fds[nr_cameras].fd = 0;
403         fds[nr_cameras].events =  POLLIN;
405         while (true) {
406                 int r = poll(fds.data(), nr_cameras + 1, -1);
407                 ASSERT(r > 0);
409                 if (fds[nr_cameras].revents != 0)
410                         break;
412                 AtomicReq req(card);
414                 for (unsigned i = 0; i < nr_cameras; i++) {
415                         if (!fds[i].revents)
416                                 continue;
417                         cameras[i]->show_next_frame(req);
418                         fds[i].revents = 0;
419                 }
421                 r = req.test();
422                 FAIL_IF(r, "Atomic commit failed: %d", r);
424                 req.commit();
425         }
427         for (auto cam : cameras)
428                 delete cam;