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