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