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>
  11
  12 #include <kms++/kms++.h>
  13 #include <kms++util/kms++util.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 class BufferProvider {
  22         DRM,
  23         V4L2,
  24 };
  25
  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();
  33
  34         CameraPipeline(const CameraPipeline& other) = delete;
  35         CameraPipeline& operator=(const CameraPipeline& other) = delete;
  36
  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 };
  54
  55 static int buffer_export(int v4lfd, enum v4l2_buf_type bt, uint32_t index, int *dmafd)
  56 {
  57         struct v4l2_exportbuffer expbuf;
  58
  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         }
  66
  67         *dmafd = expbuf.fd;
  68
  69         return 0;
  70 }
  71
  72 ExtFramebuffer* CameraPipeline::GetExtFrameBuffer(Card& card, uint32_t i, PixelFormat pixfmt)
  73 {
  74         int r, dmafd;
  75
  76         r = buffer_export(m_fd, V4L2_BUF_TYPE_VIDEO_CAPTURE, i, &dmafd);
  77         ASSERT(r == 0);
  78
  79         const PixelFormatInfo& format_info = get_pixel_format_info(pixfmt);
  80         ASSERT(format_info.num_planes == 1);
  81
  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 };
  85
  86         return new ExtFramebuffer(card, m_in_width, m_in_height, pixfmt,
  87                                   fds, pitches, offsets);
  88 }
  89
  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;
  97
  98         return false;
  99 }
 100
 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)
 105 {
 106
 107         int r;
 108         uint32_t best_w = 320;
 109         uint32_t best_h = 240;
 110
 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         };
 125
 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;
 131
 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);
 136
 137         v4lfmt.fmt.pix.pixelformat = (uint32_t)pixfmt;
 138         v4lfmt.fmt.pix.width = m_in_width;
 139         v4lfmt.fmt.pix.height = m_in_height;
 140
 141         r = ioctl(m_fd, VIDIOC_S_FMT, &v4lfmt);
 142         ASSERT(r == 0);
 143
 144         uint32_t v4l_mem;
 145
 146         if (m_buffer_provider == BufferProvider::V4L2)
 147                 v4l_mem = V4L2_MEMORY_MMAP;
 148         else
 149                 v4l_mem = V4L2_MEMORY_DMABUF;
 150
 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);
 158
 159         struct v4l2_buffer v4lbuf = { };
 160         v4lbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 161         v4lbuf.memory = v4l_mem;
 162
 163         for (unsigned i = 0; i < CAMERA_BUF_QUEUE_SIZE; i++) {
 164                 DumbFramebuffer *fb = NULL;
 165                 ExtFramebuffer *extfb = NULL;
 166
 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);
 172
 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);
 178
 179                 if (m_buffer_provider == BufferProvider::V4L2)
 180                         m_extfb.push_back(extfb);
 181                 else
 182                         m_fb.push_back(fb);
 183         }
 184
 185         m_plane = plane;
 186
 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);
 190
 191         Framebuffer *fb;
 192         if (m_buffer_provider == BufferProvider::V4L2)
 193                 fb = m_extfb[0];
 194         else
 195                 fb = m_fb[0];
 196
 197         req.add(m_plane, "CRTC_ID", m_crtc->id());
 198         req.add(m_plane, "FB_ID", fb->id());
 199
 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);
 204
 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);
 209
 210         r = req.commit_sync();
 211         FAIL_IF(r, "initial plane setup failed");
 212 }
 213
 214 CameraPipeline::~CameraPipeline()
 215 {
 216         for (unsigned i = 0; i < m_fb.size(); i++)
 217                 delete m_fb[i];
 218
 219         for (unsigned i = 0; i < m_extfb.size(); i++)
 220                 delete m_extfb[i];
 221
 222         ::close(m_fd);
 223 }
 224
 225 void CameraPipeline::start_streaming()
 226 {
 227         enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 228
 229         int r = ioctl(m_fd, VIDIOC_STREAMON, &type);
 230         FAIL_IF(r, "Failed to enable camera stream: %d", r);
 231 }
 232
 233 void CameraPipeline::show_next_frame(AtomicReq& req)
 234 {
 235         int r;
 236         uint32_t v4l_mem;
 237
 238         if (m_buffer_provider == BufferProvider::V4L2)
 239                 v4l_mem = V4L2_MEMORY_MMAP;
 240         else
 241                 v4l_mem = V4L2_MEMORY_DMABUF;
 242
 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         }
 251
 252         unsigned fb_index = v4l2buf.index;
 253
 254         Framebuffer *fb;
 255         if (m_buffer_provider == BufferProvider::V4L2)
 256                 fb = m_extfb[fb_index];
 257         else
 258                 fb = m_fb[fb_index];
 259
 260         req.add(m_plane, "FB_ID", fb->id());
 261
 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);
 271
 272         }
 273
 274         m_prev_fb_index = fb_index;
 275 }
 276
 277 static bool is_capture_dev(int fd)
 278 {
 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;
 283 }
 284
 285 std::vector<std::string> glob(const std::string& pat)
 286 {
 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;
 294 }
 295
 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                 ;
 303
 304 int main(int argc, char** argv)
 305 {
 306         BufferProvider buffer_provider = BufferProvider::DRM;
 307         bool single_cam = false;
 308
 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         };
 329
 330         optionset.parse(argc, argv);
 331
 332         if (optionset.params().size() > 0) {
 333                 puts(usage_str);
 334                 exit(-1);
 335         }
 336
 337         auto pixfmt = PixelFormat::YUYV;
 338
 339         Card card;
 340
 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");
 345
 346         vector<int> camera_fds;
 347
 348         for (string vidpath : glob("/dev/video*")) {
 349                 int fd = ::open(vidpath.c_str(), O_RDWR | O_NONBLOCK);
 350
 351                 if (fd < 0)
 352                         continue;
 353
 354                 if (!is_capture_dev(fd)) {
 355                         close(fd);
 356                         continue;
 357                 }
 358
 359                 camera_fds.push_back(fd);
 360
 361                 if (single_cam)
 362                         break;
 363         }
 364
 365         FAIL_IF(camera_fds.size() == 0, "No cameras found");
 366
 367         vector<Plane*> available_planes;
 368         for (Plane* p : crtc->get_possible_planes()) {
 369                 if (p->plane_type() != PlaneType::Overlay)
 370                         continue;
 371
 372                 if (!p->supports_format(pixfmt))
 373                         continue;
 374
 375                 available_planes.push_back(p);
 376         }
 377
 378         FAIL_IF(available_planes.size() < camera_fds.size(), "Not enough video planes for cameras");
 379
 380         uint32_t plane_w = crtc->width() / camera_fds.size();
 381         vector<CameraPipeline*> cameras;
 382
 383         for (unsigned i = 0; i < camera_fds.size(); ++i) {
 384                 int cam_fd = camera_fds[i];
 385                 Plane* plane = available_planes[i];
 386
 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         }
 391
 392         unsigned nr_cameras = cameras.size();
 393
 394         vector<pollfd> fds(nr_cameras + 1);
 395
 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;
 402
 403         for (auto cam : cameras)
 404                 cam->start_streaming();
 405
 406         while (true) {
 407                 int r = poll(fds.data(), nr_cameras + 1, -1);
 408                 ASSERT(r > 0);
 409
 410                 if (fds[nr_cameras].revents != 0)
 411                         break;
 412
 413                 AtomicReq req(card);
 414
 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                 }
 421
 422                 r = req.test();
 423                 FAIL_IF(r, "Atomic commit failed: %d", r);
 424
 425                 req.commit_sync();
 426         }
 427
 428         for (auto cam : cameras)
 429                 delete cam;
 430 }