1 #include <cstdio>
2 #include <poll.h>
3 #include <unistd.h>
4 #include <algorithm>
5 #include <fstream>
6
7 #include <kms++/kms++.h>
8 #include <kms++util/kms++util.h>
9 #include <kms++util/videodevice.h>
10
11 #define CAMERA_BUF_QUEUE_SIZE 5
12
13 using namespace std;
14 using namespace kms;
15
16 static vector<DumbFramebuffer*> s_fbs;
17 static vector<DumbFramebuffer*> s_free_fbs;
18 static vector<DumbFramebuffer*> s_wb_fbs;
19 static vector<DumbFramebuffer*> s_ready_fbs;
20
21 class WBStreamer
22 {
23 public:
WBStreamer(VideoStreamer * streamer,Crtc * crtc,PixelFormat pixfmt)24 WBStreamer(VideoStreamer* streamer, Crtc* crtc, PixelFormat pixfmt)
25 : m_capdev(*streamer)
26 {
27 Videomode m = crtc->mode();
28
29 m_capdev.set_port(crtc->idx());
30 m_capdev.set_format(pixfmt, m.hdisplay, m.vdisplay / (m.interlace() ? 2 : 1));
31 m_capdev.set_queue_size(s_fbs.size());
32
33 for (auto fb : s_free_fbs) {
34 m_capdev.queue(fb);
35 s_wb_fbs.push_back(fb);
36 }
37
38 s_free_fbs.clear();
39 }
40
~WBStreamer()41 ~WBStreamer()
42 {
43 }
44
45 WBStreamer(const WBStreamer& other) = delete;
46 WBStreamer& operator=(const WBStreamer& other) = delete;
47
fd() const48 int fd() const { return m_capdev.fd(); }
49
start_streaming()50 void start_streaming()
51 {
52 m_capdev.stream_on();
53 }
54
stop_streaming()55 void stop_streaming()
56 {
57 m_capdev.stream_off();
58 }
59
Dequeue()60 DumbFramebuffer* Dequeue()
61 {
62 auto fb = m_capdev.dequeue();
63
64 auto iter = find(s_wb_fbs.begin(), s_wb_fbs.end(), fb);
65 s_wb_fbs.erase(iter);
66
67 s_ready_fbs.insert(s_ready_fbs.begin(), fb);
68
69 return fb;
70 }
71
Queue()72 void Queue()
73 {
74 if (s_free_fbs.size() == 0)
75 return;
76
77 auto fb = s_free_fbs.back();
78 s_free_fbs.pop_back();
79
80 m_capdev.queue(fb);
81
82 s_wb_fbs.insert(s_wb_fbs.begin(), fb);
83 }
84
85 private:
86 VideoStreamer& m_capdev;
87 };
88
89 class WBFlipState : private PageFlipHandlerBase
90 {
91 public:
WBFlipState(Card & card,Crtc * crtc,Plane * plane)92 WBFlipState(Card& card, Crtc* crtc, Plane* plane)
93 : m_card(card), m_crtc(crtc), m_plane(plane)
94 {
95 auto fb = s_ready_fbs.back();
96 s_ready_fbs.pop_back();
97
98 AtomicReq req(m_card);
99
100 req.add(m_plane, "CRTC_ID", m_crtc->id());
101 req.add(m_plane, "FB_ID", fb->id());
102
103 req.add(m_plane, "CRTC_X", 0);
104 req.add(m_plane, "CRTC_Y", 0);
105 req.add(m_plane, "CRTC_W", min((uint32_t)m_crtc->mode().hdisplay, fb->width()));
106 req.add(m_plane, "CRTC_H", min((uint32_t)m_crtc->mode().vdisplay, fb->height()));
107
108 req.add(m_plane, "SRC_X", 0);
109 req.add(m_plane, "SRC_Y", 0);
110 req.add(m_plane, "SRC_W", fb->width() << 16);
111 req.add(m_plane, "SRC_H", fb->height() << 16);
112
113 int r = req.commit_sync();
114 FAIL_IF(r, "initial plane setup failed");
115
116 m_current_fb = fb;
117 }
118
queue_next()119 void queue_next()
120 {
121 if (m_queued_fb)
122 return;
123
124 if (s_ready_fbs.size() == 0)
125 return;
126
127 auto fb = s_ready_fbs.back();
128 s_ready_fbs.pop_back();
129
130 AtomicReq req(m_card);
131 req.add(m_plane, "FB_ID", fb->id());
132
133 int r = req.commit(this);
134 if (r)
135 EXIT("Flip commit failed: %d\n", r);
136
137 m_queued_fb = fb;
138 }
139
140 private:
handle_page_flip(uint32_t frame,double time)141 void handle_page_flip(uint32_t frame, double time)
142 {
143 if (m_queued_fb) {
144 if (m_current_fb)
145 s_free_fbs.insert(s_free_fbs.begin(), m_current_fb);
146
147 m_current_fb = m_queued_fb;
148 m_queued_fb = nullptr;
149 }
150
151 queue_next();
152 }
153
154 Card& m_card;
155 Crtc* m_crtc;
156 Plane* m_plane;
157
158 DumbFramebuffer* m_current_fb = nullptr;
159 DumbFramebuffer* m_queued_fb = nullptr;
160 };
161
162 class BarFlipState : private PageFlipHandlerBase
163 {
164 public:
BarFlipState(Card & card,Crtc * crtc,Plane * plane,uint32_t width,uint32_t height)165 BarFlipState(Card& card, Crtc* crtc, Plane* plane, uint32_t width, uint32_t height)
166 : m_card(card), m_crtc(crtc), m_plane(plane)
167 {
168 for (unsigned i = 0; i < s_num_buffers; ++i)
169 m_fbs[i] = new DumbFramebuffer(card, width, height, PixelFormat::XRGB8888);
170 }
171
~BarFlipState()172 ~BarFlipState()
173 {
174 for (unsigned i = 0; i < s_num_buffers; ++i)
175 delete m_fbs[i];
176 }
177
start_flipping()178 void start_flipping()
179 {
180 m_frame_num = 0;
181 queue_next();
182 }
183
184 private:
handle_page_flip(uint32_t frame,double time)185 void handle_page_flip(uint32_t frame, double time)
186 {
187 m_frame_num++;
188 queue_next();
189 }
190
get_bar_pos(DumbFramebuffer * fb,unsigned frame_num)191 static unsigned get_bar_pos(DumbFramebuffer* fb, unsigned frame_num)
192 {
193 return (frame_num * bar_speed) % (fb->width() - bar_width + 1);
194 }
195
draw_bar(DumbFramebuffer * fb,unsigned frame_num)196 void draw_bar(DumbFramebuffer* fb, unsigned frame_num)
197 {
198 int old_xpos = frame_num < s_num_buffers ? -1 : get_bar_pos(fb, frame_num - s_num_buffers);
199 int new_xpos = get_bar_pos(fb, frame_num);
200
201 draw_color_bar(*fb, old_xpos, new_xpos, bar_width);
202 draw_text(*fb, fb->width() / 2, 0, to_string(frame_num), RGB(255, 255, 255));
203 }
204
queue_next()205 void queue_next()
206 {
207 AtomicReq req(m_card);
208
209 unsigned cur = m_frame_num % s_num_buffers;
210
211 auto fb = m_fbs[cur];
212
213 draw_bar(fb, m_frame_num);
214
215 req.add(m_plane, {
216 { "CRTC_ID", m_crtc->id() },
217 { "FB_ID", fb->id() },
218
219 { "CRTC_X", 0 },
220 { "CRTC_Y", 0 },
221 { "CRTC_W", min((uint32_t)m_crtc->mode().hdisplay, fb->width()) },
222 { "CRTC_H", min((uint32_t)m_crtc->mode().vdisplay, fb->height()) },
223
224 { "SRC_X", 0 },
225 { "SRC_Y", 0 },
226 { "SRC_W", fb->width() << 16 },
227 { "SRC_H", fb->height() << 16 },
228 });
229
230 int r = req.commit(this);
231 if (r)
232 EXIT("Flip commit failed: %d\n", r);
233 }
234
235 static const unsigned s_num_buffers = 3;
236
237 DumbFramebuffer* m_fbs[s_num_buffers];
238
239 Card& m_card;
240 Crtc* m_crtc;
241 Plane* m_plane;
242
243 unsigned m_frame_num;
244
245 static const unsigned bar_width = 20;
246 static const unsigned bar_speed = 8;
247 };
248
249 static const char* usage_str =
250 "Usage: wbcap [OPTIONS]\n\n"
251 "Options:\n"
252 " -s, --src=CONN Source connector\n"
253 " -d, --dst=CONN Destination connector\n"
254 " -m, --smode=MODE Source connector videomode\n"
255 " -M, --dmode=MODE Destination connector videomode\n"
256 " -f, --format=4CC Format\n"
257 " -w, --write Write captured frames to wbcap.raw file\n"
258 " -h, --help Print this help\n"
259 ;
260
main(int argc,char ** argv)261 int main(int argc, char** argv)
262 {
263 string src_conn_name;
264 string src_mode_name;
265 string dst_conn_name;
266 string dst_mode_name;
267 PixelFormat pixfmt = PixelFormat::XRGB8888;
268 bool write_file = false;
269
270 OptionSet optionset = {
271 Option("s|src=", [&](string s)
272 {
273 src_conn_name = s;
274 }),
275 Option("m|smode=", [&](string s)
276 {
277 src_mode_name = s;
278 }),
279 Option("d|dst=", [&](string s)
280 {
281 dst_conn_name = s;
282 }),
283 Option("M|dmode=", [&](string s)
284 {
285 dst_mode_name = s;
286 }),
287 Option("f|format=", [&](string s)
288 {
289 pixfmt = FourCCToPixelFormat(s);
290 }),
291 Option("w|write", [&]()
292 {
293 write_file = true;
294 }),
295 Option("h|help", [&]()
296 {
297 puts(usage_str);
298 exit(-1);
299 }),
300 };
301
302 optionset.parse(argc, argv);
303
304 if (optionset.params().size() > 0) {
305 puts(usage_str);
306 exit(-1);
307 }
308
309 if (src_conn_name.empty())
310 EXIT("No source connector defined");
311
312 if (dst_conn_name.empty())
313 EXIT("No destination connector defined");
314
315 VideoDevice vid("/dev/video11");
316
317 Card card;
318 ResourceManager resman(card);
319
320 card.disable_all();
321
322 auto src_conn = resman.reserve_connector(src_conn_name);
323 auto src_crtc = resman.reserve_crtc(src_conn);
324 auto src_plane = resman.reserve_generic_plane(src_crtc, pixfmt);
325 FAIL_IF(!src_plane, "Plane not found");
326 Videomode src_mode = src_mode_name.empty() ? src_conn->get_default_mode() : src_conn->get_mode(src_mode_name);
327 src_crtc->set_mode(src_conn, src_mode);
328
329
330 auto dst_conn = resman.reserve_connector(dst_conn_name);
331 auto dst_crtc = resman.reserve_crtc(dst_conn);
332 auto dst_plane = resman.reserve_overlay_plane(dst_crtc, pixfmt);
333 FAIL_IF(!dst_plane, "Plane not found");
334 Videomode dst_mode = dst_mode_name.empty() ? dst_conn->get_default_mode() : dst_conn->get_mode(dst_mode_name);
335 dst_crtc->set_mode(dst_conn, dst_mode);
336
337 uint32_t src_width = src_mode.hdisplay;
338 uint32_t src_height = src_mode.vdisplay;
339
340 uint32_t dst_width = src_mode.hdisplay;
341 uint32_t dst_height = src_mode.vdisplay;
342 if (src_mode.interlace())
343 dst_height /= 2;
344
345 printf("src %s, crtc %s\n", src_conn->fullname().c_str(), src_mode.to_string().c_str());
346
347 printf("dst %s, crtc %s\n", dst_conn->fullname().c_str(), dst_mode.to_string().c_str());
348
349 printf("src_fb %ux%u, dst_fb %ux%u\n", src_width, src_height, dst_width, dst_height);
350
351 for (int i = 0; i < CAMERA_BUF_QUEUE_SIZE; ++i) {
352 auto fb = new DumbFramebuffer(card, dst_width, dst_height, pixfmt);
353 s_fbs.push_back(fb);
354 s_free_fbs.push_back(fb);
355 }
356
357 // get one fb for initial setup
358 s_ready_fbs.push_back(s_free_fbs.back());
359 s_free_fbs.pop_back();
360
361 // This draws a moving bar to SRC display
362 BarFlipState barflipper(card, src_crtc, src_plane, src_width, src_height);
363 barflipper.start_flipping();
364
365 // This shows the captured SRC frames on DST display
366 WBFlipState wbflipper(card, dst_crtc, dst_plane);
367
368 WBStreamer wb(vid.get_capture_streamer(), src_crtc, pixfmt);
369 wb.start_streaming();
370
371 vector<pollfd> fds(3);
372
373 fds[0].fd = 0;
374 fds[0].events = POLLIN;
375 fds[1].fd = wb.fd();
376 fds[1].events = POLLIN;
377 fds[2].fd = card.fd();
378 fds[2].events = POLLIN;
379
380 uint32_t dst_frame_num = 0;
381
382 const string filename = "wbcap.raw";
383 unique_ptr<ofstream> os;
384 if (write_file)
385 os = unique_ptr<ofstream>(new ofstream(filename, ofstream::binary));
386
387 while (true) {
388 int r = poll(fds.data(), fds.size(), -1);
389 ASSERT(r > 0);
390
391 if (fds[0].revents != 0)
392 break;
393
394 if (fds[1].revents) {
395 fds[1].revents = 0;
396
397 DumbFramebuffer* fb = wb.Dequeue();
398
399 if (write_file) {
400 printf("Writing frame %u to %s\n", dst_frame_num, filename.c_str());
401
402 for (unsigned i = 0; i < fb->num_planes(); ++i)
403 os->write((char*)fb->map(i), fb->size(i));
404
405 dst_frame_num++;
406 }
407
408 wbflipper.queue_next();
409 }
410
411 if (fds[2].revents) {
412 fds[2].revents = 0;
413
414 card.call_page_flip_handlers();
415 wb.Queue();
416 }
417 }
418
419 printf("exiting...\n");
420 }
421