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
  11 #include "kms++.h"
  12 #include "test.h"
  13 #include "opts.h"
  14
  15 #define CAMERA_BUF_QUEUE_SIZE   3
  16 #define MAX_CAMERA              9
  17
  18 using namespace std;
  19 using namespace kms;
  20
  21 enum buffer_provider {
  22         BUFFER_DRM = 0,
  23         BUFFER_V4L,
  24 };
  25
  26 class Camera
  27 {
  28 public:
  29         Camera(int camera_id, Card& card, Plane* plane, uint32_t x, uint32_t y,
  30                uint32_t iw, uint32_t ih, PixelFormat pixfmt,
  31                enum buffer_provider buffer_type);
  32         ~Camera();
  33
  34         Camera(const Camera& other) = delete;
  35         Camera& operator=(const Camera& other) = delete;
  36
  37         void show_next_frame(Crtc* crtc);
  38         int fd() const { return m_fd; }
  39 private:
  40         ExtFramebuffer* GetExtFrameBuffer(Card& card, int i, PixelFormat pixfmt);
  41         int m_fd;       /* camera file descriptor */
  42         Plane* m_plane;
  43         enum buffer_provider m_buffer_type;
  44         vector<DumbFramebuffer*> m_fb; /* framebuffers for DRM buffers */
  45         vector<ExtFramebuffer*> m_extfb; /* framebuffers for V4L2 buffers */
  46         int m_prev_fb_index;
  47         uint32_t m_in_width, m_in_height; /* camera capture resolution */
  48         /* image properties for display */
  49         uint32_t m_out_width, m_out_height;
  50         uint32_t m_out_x, m_out_y;
  51 };
  52
  53 static int buffer_export(int v4lfd, enum v4l2_buf_type bt, int index, int *dmafd)
  54 {
  55         struct v4l2_exportbuffer expbuf;
  56
  57         memset(&expbuf, 0, sizeof(expbuf));
  58         expbuf.type = bt;
  59         expbuf.index = index;
  60         if (ioctl(v4lfd, VIDIOC_EXPBUF, &expbuf) == -1) {
  61                 perror("VIDIOC_EXPBUF");
  62                 return -1;
  63         }
  64
  65         *dmafd = expbuf.fd;
  66
  67         return 0;
  68 }
  69
  70 ExtFramebuffer* Camera::GetExtFrameBuffer(Card& card, int i, PixelFormat pixfmt)
  71 {
  72         int r, dmafd;
  73
  74         r = buffer_export(m_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, i, &dmafd);
  75         ASSERT(r == 0);
  76
  77         uint32_t handle;
  78         r = drmPrimeFDToHandle(card.fd(), dmafd, &handle);
  79         ASSERT(r == 0);
  80
  81         const PixelFormatInfo& format_info = get_pixel_format_info(pixfmt);
  82         ASSERT(format_info.num_planes == 1);
  83
  84         uint32_t handles[4] { handle };
  85         uint32_t pitches[4] { m_in_width * (format_info.planes[0].bitspp / 8) };
  86         uint32_t offsets[4] { };
  87
  88         return new ExtFramebuffer(card, m_in_width, m_in_height, pixfmt,
  89                                   handles, pitches, offsets);
  90 }
  91
  92 bool inline better_size(struct v4l2_frmsize_discrete* v4ldisc,
  93                         uint32_t iw, uint32_t ih,
  94                         uint32_t best_w, uint32_t best_h)
  95 {
  96         if (v4ldisc->width <= iw && v4ldisc->height <= ih &&
  97             (v4ldisc->width >= best_w || v4ldisc->height >= best_h))
  98                 return true;
  99
 100         return false;
 101 }
 102
 103 Camera::Camera(int camera_id, Card& card, Plane* plane, uint32_t x, uint32_t y,
 104                uint32_t iw, uint32_t ih, PixelFormat pixfmt,
 105                enum buffer_provider buffer_type)
 106 {
 107         char dev_name[20];
 108         int r, i;
 109         uint32_t best_w = 320;
 110         uint32_t best_h = 240;
 111         uint32_t v4l_mem;
 112
 113         m_buffer_type = buffer_type;
 114         if (m_buffer_type == BUFFER_V4L)
 115                 v4l_mem = V4L2_MEMORY_MMAP;
 116         else
 117                 v4l_mem = V4L2_MEMORY_DMABUF;
 118
 119         sprintf(dev_name, "/dev/video%d", camera_id);
 120         m_fd = ::open(dev_name, O_RDWR | O_NONBLOCK);
 121
 122         ASSERT(m_fd >= 0);
 123
 124         struct v4l2_frmsizeenum v4lfrms = { };
 125         v4lfrms.pixel_format = (uint32_t) pixfmt;
 126         while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
 127                 if (v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE) {
 128                         if (better_size(&v4lfrms.discrete, iw, ih,
 129                                         best_w, best_h)) {
 130                                 best_w = v4lfrms.discrete.width;
 131                                 best_h = v4lfrms.discrete.height;
 132                         }
 133                 } else {
 134                         break;
 135                 }
 136                 v4lfrms.index++;
 137         };
 138
 139         m_out_width = m_in_width = best_w;
 140         m_out_height = m_in_height = best_h;
 141         /* Move it to the middle of the requested area */
 142         m_out_x = x + iw / 2 - m_out_width / 2;
 143         m_out_y = y + ih / 2 - m_out_height / 2;
 144
 145         struct v4l2_format v4lfmt = { };
 146         v4lfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 147         r = ioctl(m_fd, VIDIOC_G_FMT, &v4lfmt);
 148         ASSERT(r == 0);
 149
 150         v4lfmt.fmt.pix.pixelformat = (uint32_t) pixfmt;
 151         v4lfmt.fmt.pix.width = m_in_width;
 152         v4lfmt.fmt.pix.height = m_in_height;
 153
 154         r = ioctl(m_fd, VIDIOC_S_FMT, &v4lfmt);
 155         ASSERT(r == 0);
 156
 157         struct v4l2_requestbuffers v4lreqbuf = { };
 158         v4lreqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 159         v4lreqbuf.memory = v4l_mem;
 160         v4lreqbuf.count = CAMERA_BUF_QUEUE_SIZE;
 161         r = ioctl(m_fd, VIDIOC_REQBUFS, &v4lreqbuf);
 162         ASSERT(r == 0);
 163         ASSERT(v4lreqbuf.count == CAMERA_BUF_QUEUE_SIZE);
 164
 165         struct v4l2_buffer v4lbuf = { };
 166         v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 167         v4lbuf.memory = v4l_mem;
 168
 169         for (i = 0; i < CAMERA_BUF_QUEUE_SIZE; i++) {
 170                 DumbFramebuffer *fb = NULL;
 171                 ExtFramebuffer *extfb = NULL;
 172
 173                 if (m_buffer_type == BUFFER_V4L)
 174                         extfb = GetExtFrameBuffer(card, i, pixfmt);
 175                 else
 176                         fb = new DumbFramebuffer(card, m_in_width,
 177                                                  m_in_height, pixfmt);
 178
 179                 v4lbuf.index = i;
 180                 if (m_buffer_type == BUFFER_DRM)
 181                         v4lbuf.m.fd = fb->prime_fd(0);
 182                 r = ioctl(m_fd, VIDIOC_QBUF, &v4lbuf);
 183                 ASSERT(r == 0);
 184
 185                 if (m_buffer_type == BUFFER_V4L)
 186                         m_extfb.push_back(extfb);
 187                 else
 188                         m_fb.push_back(fb);
 189         }
 190         m_prev_fb_index = -1;
 191         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 192
 193         r = ioctl(m_fd, VIDIOC_STREAMON, &type);
 194                 ASSERT(r == 0);
 195
 196         m_plane = plane;
 197 }
 198
 199
 200 Camera::~Camera()
 201 {
 202         for (unsigned i = 0; i < m_fb.size(); i++)
 203                 delete m_fb[i];
 204
 205         for (unsigned i = 0; i < m_extfb.size(); i++)
 206                 delete m_extfb[i];
 207
 208         ::close(m_fd);
 209 }
 210
 211 void Camera::show_next_frame(Crtc* crtc)
 212 {
 213         int r;
 214         int fb_index;
 215         uint32_t v4l_mem;
 216
 217         if (m_buffer_type == BUFFER_V4L)
 218                 v4l_mem = V4L2_MEMORY_MMAP;
 219         else
 220                 v4l_mem = V4L2_MEMORY_DMABUF;
 221
 222         struct v4l2_buffer v4l2buf = { };
 223         v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 224         v4l2buf.memory = v4l_mem;
 225         r = ioctl(m_fd, VIDIOC_DQBUF, &v4l2buf);
 226         if (r != 0) {
 227                 printf("VIDIOC_DQBUF ioctl failed with %d\n", errno);
 228                 return;
 229         }
 230
 231         fb_index = v4l2buf.index;
 232         if (m_buffer_type == BUFFER_V4L)
 233                 r = crtc->set_plane(m_plane, *m_extfb[fb_index],
 234                                     m_out_x, m_out_y, m_out_width, m_out_height,
 235                                     0, 0, m_in_width, m_in_height);
 236         else
 237                 r = crtc->set_plane(m_plane, *m_fb[fb_index],
 238                                     m_out_x, m_out_y, m_out_width, m_out_height,
 239                                     0, 0, m_in_width, m_in_height);
 240
 241         ASSERT(r == 0);
 242
 243         if (m_prev_fb_index >= 0) {
 244                 memset(&v4l2buf, 0, sizeof(v4l2buf));
 245                 v4l2buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 246                 v4l2buf.memory = v4l_mem;
 247                 v4l2buf.index = m_prev_fb_index;
 248                 if (m_buffer_type == BUFFER_DRM)
 249                         v4l2buf.m.fd = m_fb[m_prev_fb_index]->prime_fd(0);
 250                 r = ioctl(m_fd, VIDIOC_QBUF, &v4l2buf);
 251                 ASSERT(r == 0);
 252
 253         }
 254         m_prev_fb_index = fb_index;
 255 }
 256
 257
 258 static bool is_capture_dev(int fd)
 259 {
 260         struct v4l2_capability cap = { };
 261         int r;
 262
 263         r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
 264         ASSERT(r == 0);
 265         if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
 266                 return false;
 267
 268         return true;
 269 }
 270
 271 static vector<int> count_cameras()
 272 {
 273         int i, fd;
 274         vector<int> camera_idx;
 275         char dev_name[20];
 276
 277         for (i = 0; i < MAX_CAMERA; i++) {
 278                 sprintf(dev_name, "/dev/video%d", i);
 279                 fd = ::open(dev_name, O_RDWR | O_NONBLOCK);
 280                 if (fd >= 0) {
 281                         if (is_capture_dev(fd))
 282                                 camera_idx.push_back(i);
 283                         close(fd);
 284                 }
 285         }
 286
 287         return camera_idx;
 288 }
 289
 290 static const char* usage_str =
 291 "Usage: kmscapture [OPTIONS]\n\n"
 292 "Options:\n"
 293 "  -s, --single                Single camera mode. Open only /dev/video0\n"
 294 "      --buffer-type=<drm|v4l> Use DRM or V4L provided buffers. Default: DRM\n"
 295 "  -h, --help                  Print this help\n"
 296 ;
 297
 298 int main(int argc, char** argv)
 299 {
 300         uint32_t w;
 301         enum buffer_provider buffer_type = BUFFER_DRM;
 302         int i;
 303
 304         auto camera_idx = count_cameras();
 305         int nr_cameras = camera_idx.size();
 306
 307         FAIL_IF(!nr_cameras, "Not a single camera has been found.");
 308
 309         OptionSet optionset = {
 310                 Option("s|single", [&](string s)
 311                 {
 312                         nr_cameras = 1;
 313                 }),
 314                 Option("|buffer-type=", [&](string s)
 315                 {
 316                         if (!s.compare("v4l"))
 317                                 buffer_type = BUFFER_V4L;
 318                         else if (s.compare("drm"))
 319                                 printf("invalid buffer-type: %s\n", s.c_str());
 320                 }),
 321                 Option("h|help", [&]()
 322                 {
 323                         puts(usage_str);
 324                         exit(-1);
 325                 }),
 326         };
 327
 328         optionset.parse(argc, argv);
 329
 330         if (optionset.params().size() > 0) {
 331                 puts(usage_str);
 332                 exit(-1);
 333         }
 334
 335         auto pixfmt = FourCCToPixelFormat("YUYV");
 336
 337         Card card;
 338
 339         auto conn = card.get_first_connected_connector();
 340         auto crtc = conn->get_current_crtc();
 341         printf("Display: %dx%d\n", crtc->width(), crtc->height());
 342         printf("Buffer provider: %s\n",
 343                buffer_type == BUFFER_V4L? "V4L" : "DRM");
 344
 345         w = crtc->width() / nr_cameras;
 346         vector<Camera*> cameras;
 347         i = 0;
 348         for (Plane* p : crtc->get_possible_planes()) {
 349                 if (p->plane_type() != PlaneType::Overlay)
 350                         continue;
 351
 352                 if (!p->supports_format(pixfmt))
 353                         continue;
 354
 355                 auto cam = new Camera(camera_idx[i], card, p, i * w, 0,
 356                                       w, crtc->height(), pixfmt, buffer_type);
 357                 cameras.push_back(cam);
 358                 if (++i == nr_cameras)
 359                         break;
 360         }
 361
 362         FAIL_IF(i < nr_cameras, "available plane not found");
 363
 364         struct pollfd fds[nr_cameras + 1] = { };
 365         for (i = 0; i < nr_cameras; i++) {
 366                 fds[i].fd = cameras[i]->fd();
 367                 fds[i].events =  POLLIN;
 368         }
 369         fds[nr_cameras].fd = 0;
 370         fds[nr_cameras].events =  POLLIN;
 371
 372         while (true) {
 373                 int r = poll(fds, nr_cameras + 1, -1);
 374                 ASSERT(r > 0);
 375
 376                 if (fds[nr_cameras].revents != 0)
 377                         break;
 378
 379                 for (i = 0; i < nr_cameras; i++) {
 380                         if (!fds[i].revents)
 381                                 continue;
 382                         cameras[i]->show_next_frame(crtc);
 383                         fds[i].revents = 0;
 384                 }
 385         }
 386
 387         for (auto cam : cameras)
 388                 delete cam;
 389 }