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 } 462
