kms++util/src/videodevice.cpp

   1 #include <string>
   2
   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>
   9
  10 #include <kms++/kms++.h>
  11 #include <kms++util/kms++util.h>
  12 #include <kms++util/videodevice.h>
  13
  14 using namespace std;
  15 using namespace kms;
  16
  17 /* V4L2 helper funcs */
  18 static vector<PixelFormat> v4l2_get_formats(int fd, uint32_t buf_type)
  19 {
  20         vector<PixelFormat> v;
  21
  22         v4l2_fmtdesc desc { };
  23         desc.type = buf_type;
  24
  25         while (ioctl(fd, VIDIOC_ENUM_FMT, &desc) == 0) {
  26                 v.push_back((PixelFormat)desc.pixelformat);
  27                 desc.index++;
  28         }
  29
  30         return v;
  31 }
  32
  33 static void v4l2_set_format(int fd, PixelFormat fmt, uint32_t width, uint32_t height, uint32_t buf_type)
  34 {
  35         int r;
  36
  37         v4l2_format v4lfmt { };
  38
  39         v4lfmt.type = buf_type;
  40         r = ioctl(fd, VIDIOC_G_FMT, &v4lfmt);
  41         ASSERT(r == 0);
  42
  43         const PixelFormatInfo& pfi = get_pixel_format_info(fmt);
  44
  45         bool mplane = buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE || buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
  46
  47         if (mplane) {
  48                 v4l2_pix_format_mplane& mp = v4lfmt.fmt.pix_mp;
  49
  50                 mp.pixelformat = (uint32_t)fmt;
  51                 mp.width = width;
  52                 mp.height = height;
  53
  54                 mp.num_planes = pfi.num_planes;
  55
  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];
  59
  60                         p.bytesperline = width * pfpi.bitspp / 8;
  61                         p.sizeimage = p.bytesperline * height / pfpi.ysub;
  62                 }
  63
  64                 r = ioctl(fd, VIDIOC_S_FMT, &v4lfmt);
  65                 ASSERT(r == 0);
  66
  67                 ASSERT(mp.pixelformat == (uint32_t)fmt);
  68                 ASSERT(mp.width == width);
  69                 ASSERT(mp.height == height);
  70
  71                 ASSERT(mp.num_planes == pfi.num_planes);
  72
  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];
  76
  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);
  82
  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;
  87
  88                 r = ioctl(fd, VIDIOC_S_FMT, &v4lfmt);
  89                 ASSERT(r == 0);
  90
  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 }
  97
  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);
 107 }
 108
 109 static void v4l2_queue_dmabuf(int fd, uint32_t index, DumbFramebuffer* fb, uint32_t buf_type)
 110 {
 111         v4l2_buffer buf { };
 112         buf.type = buf_type;
 113         buf.memory = V4L2_MEMORY_DMABUF;
 114         buf.index = index;
 115
 116         const PixelFormatInfo& pfi = get_pixel_format_info(fb->format());
 117
 118         bool mplane = buf_type == V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE || buf_type == V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
 119
 120         if (mplane) {
 121                 buf.length = pfi.num_planes;
 122
 123                 v4l2_plane planes[4] { };
 124                 buf.m.planes = planes;
 125
 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                 }
 131
 132                 int r = ioctl(fd, VIDIOC_QBUF, &buf);
 133                 ASSERT(r == 0);
 134         } else {
 135                 buf.m.fd = fb->prime_fd(0);
 136
 137                 int r = ioctl(fd, VIDIOC_QBUF, &buf);
 138                 ASSERT(r == 0);
 139         }
 140 }
 141
 142 static uint32_t v4l2_dequeue(int fd, uint32_t buf_type)
 143 {
 144         v4l2_buffer buf { };
 145         buf.type = buf_type;
 146         buf.memory = V4L2_MEMORY_DMABUF;
 147
 148         // V4L2 crashes if planes are not set
 149         v4l2_plane planes[4] { };
 150         buf.m.planes = planes;
 151         buf.length = 4;
 152
 153         int r = ioctl(fd, VIDIOC_DQBUF, &buf);
 154         if (r)
 155                 throw system_error(errno, generic_category());
 156
 157         return buf.index;
 158 }
 159
 160
 161
 162
 163 VideoDevice::VideoDevice(const string& dev)
 164         :VideoDevice(::open(dev.c_str(), O_RDWR | O_NONBLOCK))
 165 {
 166 }
 167
 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)
 170 {
 171         FAIL_IF(fd < 0, "Bad fd");
 172
 173         struct v4l2_capability cap = { };
 174         int r = ioctl(fd, VIDIOC_QUERYCAP, &cap);
 175         ASSERT(r == 0);
 176
 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         }
 184
 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         }
 192
 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         }
 208 }
 209
 210 VideoDevice::~VideoDevice()
 211 {
 212         ::close(m_fd);
 213 }
 214
 215 VideoStreamer* VideoDevice::get_capture_streamer()
 216 {
 217         ASSERT(m_has_capture);
 218
 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         }
 223
 224         return m_capture_streamer;
 225 }
 226
 227 VideoStreamer* VideoDevice::get_output_streamer()
 228 {
 229         ASSERT(m_has_output);
 230
 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         }
 235
 236         return m_output_streamer;
 237 }
 238
 239 vector<tuple<uint32_t, uint32_t>> VideoDevice::get_discrete_frame_sizes(PixelFormat fmt)
 240 {
 241         vector<tuple<uint32_t, uint32_t>> v;
 242
 243         v4l2_frmsizeenum v4lfrms { };
 244         v4lfrms.pixel_format = (uint32_t)fmt;
 245
 246         int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
 247         ASSERT(r);
 248
 249         FAIL_IF(v4lfrms.type != V4L2_FRMSIZE_TYPE_DISCRETE, "No discrete frame sizes");
 250
 251         while (ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms) == 0) {
 252                 v.emplace_back(v4lfrms.discrete.width, v4lfrms.discrete.height);
 253                 v4lfrms.index++;
 254         };
 255
 256         return v;
 257 }
 258
 259 VideoDevice::VideoFrameSize VideoDevice::get_frame_sizes(PixelFormat fmt)
 260 {
 261         v4l2_frmsizeenum v4lfrms { };
 262         v4lfrms.pixel_format = (uint32_t)fmt;
 263
 264         int r = ioctl(m_fd, VIDIOC_ENUM_FRAMESIZES, &v4lfrms);
 265         ASSERT(r);
 266
 267         FAIL_IF(v4lfrms.type == V4L2_FRMSIZE_TYPE_DISCRETE, "No continuous frame sizes");
 268
 269         VideoFrameSize s;
 270
 271         s.min_w = v4lfrms.stepwise.min_width;
 272         s.max_w = v4lfrms.stepwise.max_width;
 273         s.step_w = v4lfrms.stepwise.step_width;
 274
 275         s.min_h = v4lfrms.stepwise.min_height;
 276         s.max_h = v4lfrms.stepwise.max_height;
 277         s.step_h = v4lfrms.stepwise.step_height;
 278
 279         return s;
 280 }
 281
 282 vector<string> VideoDevice::get_capture_devices()
 283 {
 284         vector<string> v;
 285
 286         for (int i = 0; i < 20; ++i) {
 287                 string name = "/dev/video" + to_string(i);
 288
 289                 struct stat buffer;
 290                 if (stat(name.c_str(), &buffer) != 0)
 291                         continue;
 292
 293                 VideoDevice vid(name);
 294
 295                 if (vid.has_capture() && !vid.has_m2m())
 296                         v.push_back(name);
 297         }
 298
 299         return v;
 300 }
 301
 302 vector<string> VideoDevice::get_m2m_devices()
 303 {
 304         vector<string> v;
 305
 306         for (int i = 0; i < 20; ++i) {
 307                 string name = "/dev/video" + to_string(i);
 308
 309                 struct stat buffer;
 310                 if (stat(name.c_str(), &buffer) != 0)
 311                         continue;
 312
 313                 VideoDevice vid(name);
 314
 315                 if (vid.has_m2m())
 316                         v.push_back(name);
 317         }
 318
 319         return v;
 320 }
 321
 322
 323 VideoStreamer::VideoStreamer(int fd, StreamerType type)
 324         : m_fd(fd), m_type(type)
 325 {
 326
 327 }
 328
 329 std::vector<string> VideoStreamer::get_ports()
 330 {
 331         vector<string> v;
 332
 333         switch (m_type) {
 334         case StreamerType::CaptureSingle:
 335         case StreamerType::CaptureMulti:
 336         {
 337                 struct v4l2_input input { };
 338
 339                 while (ioctl(m_fd, VIDIOC_ENUMINPUT, &input) == 0) {
 340                         v.push_back(string((char*)&input.name));
 341                         input.index++;
 342                 }
 343
 344                 break;
 345         }
 346
 347         case StreamerType::OutputSingle:
 348         case StreamerType::OutputMulti:
 349         {
 350                 struct v4l2_output output { };
 351
 352                 while (ioctl(m_fd, VIDIOC_ENUMOUTPUT, &output) == 0) {
 353                         v.push_back(string((char*)&output.name));
 354                         output.index++;
 355                 }
 356
 357                 break;
 358         }
 359
 360         default:
 361                 FAIL("Bad StreamerType");
 362         }
 363
 364         return v;
 365 }
 366
 367 void VideoStreamer::set_port(uint32_t index)
 368 {
 369         unsigned long req;
 370
 371         switch (m_type) {
 372         case StreamerType::CaptureSingle:
 373         case StreamerType::CaptureMulti:
 374                 req = VIDIOC_S_INPUT;
 375                 break;
 376
 377         case StreamerType::OutputSingle:
 378         case StreamerType::OutputMulti:
 379                 req = VIDIOC_S_OUTPUT;
 380                 break;
 381
 382         default:
 383                 FAIL("Bad StreamerType");
 384         }
 385
 386         int r = ioctl(m_fd, req, &index);
 387         ASSERT(r == 0);
 388 }
 389
 390 static v4l2_buf_type get_buf_type(VideoStreamer::StreamerType type)
 391 {
 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         }
 404 }
 405
 406 std::vector<PixelFormat> VideoStreamer::get_formats()
 407 {
 408         return v4l2_get_formats(m_fd, get_buf_type(m_type));
 409 }
 410
 411 void VideoStreamer::set_format(PixelFormat fmt, uint32_t width, uint32_t height)
 412 {
 413         v4l2_set_format(m_fd, fmt, width, height, get_buf_type(m_type));
 414 }
 415
 416 void VideoStreamer::set_queue_size(uint32_t queue_size)
 417 {
 418         v4l2_request_bufs(m_fd, queue_size, get_buf_type(m_type));
 419         m_fbs.resize(queue_size);
 420 }
 421
 422 void VideoStreamer::queue(DumbFramebuffer* fb)
 423 {
 424         uint32_t idx;
 425
 426         for (idx = 0; idx < m_fbs.size(); ++idx) {
 427                 if (m_fbs[idx] == nullptr)
 428                         break;
 429         }
 430
 431         FAIL_IF(idx == m_fbs.size(), "queue full");
 432
 433         m_fbs[idx] = fb;
 434
 435         v4l2_queue_dmabuf(m_fd, idx, fb, get_buf_type(m_type));
 436 }
 437
 438 DumbFramebuffer*VideoStreamer::dequeue()
 439 {
 440         uint32_t idx = v4l2_dequeue(m_fd, get_buf_type(m_type));
 441
 442         auto fb = m_fbs[idx];
 443         m_fbs[idx] = nullptr;
 444
 445         return fb;
 446 }
 447
 448 void VideoStreamer::stream_on()
 449 {
 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);
 453 }
 454
 455 void VideoStreamer::stream_off()
 456 {
 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);
 460 }