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
main(int argc,char ** argv)260 int main(int argc, char** argv)
261 {
262 string src_conn_name;
263 string src_mode_name;
264 string dst_conn_name;
265 string dst_mode_name;
266 PixelFormat pixfmt = PixelFormat::XRGB8888;
267 bool write_file = false;
268
269 OptionSet optionset = {
270 Option("s|src=", [&](string s) {
271 src_conn_name = s;
272 }),
273 Option("m|smode=", [&](string s) {
274 src_mode_name = s;
275 }),
276 Option("d|dst=", [&](string s) {
277 dst_conn_name = s;
278 }),
279 Option("M|dmode=", [&](string s) {
280 dst_mode_name = s;
281 }),
282 Option("f|format=", [&](string s) {
283 pixfmt = FourCCToPixelFormat(s);
284 }),
285 Option("w|write", [&]() {
286 write_file = true;
287 }),
288 Option("h|help", [&]() {
289 puts(usage_str);
290 exit(-1);
291 }),
292 };
293
294 optionset.parse(argc, argv);
295
296 if (optionset.params().size() > 0) {
297 puts(usage_str);
298 exit(-1);
299 }
300
301 if (src_conn_name.empty())
302 EXIT("No source connector defined");
303
304 if (dst_conn_name.empty())
305 EXIT("No destination connector defined");
306
307 VideoDevice vid("/dev/video11");
308
309 Card card;
310 ResourceManager resman(card);
311
312 card.disable_all();
313
314 auto src_conn = resman.reserve_connector(src_conn_name);
315 auto src_crtc = resman.reserve_crtc(src_conn);
316 auto src_plane = resman.reserve_generic_plane(src_crtc, pixfmt);
317 FAIL_IF(!src_plane, "Plane not found");
318 Videomode src_mode = src_mode_name.empty() ? src_conn->get_default_mode() : src_conn->get_mode(src_mode_name);
319 src_crtc->set_mode(src_conn, src_mode);
320
321 auto dst_conn = resman.reserve_connector(dst_conn_name);
322 auto dst_crtc = resman.reserve_crtc(dst_conn);
323 auto dst_plane = resman.reserve_overlay_plane(dst_crtc, pixfmt);
324 FAIL_IF(!dst_plane, "Plane not found");
325 Videomode dst_mode = dst_mode_name.empty() ? dst_conn->get_default_mode() : dst_conn->get_mode(dst_mode_name);
326 dst_crtc->set_mode(dst_conn, dst_mode);
327
328 uint32_t src_width = src_mode.hdisplay;
329 uint32_t src_height = src_mode.vdisplay;
330
331 uint32_t dst_width = src_mode.hdisplay;
332 uint32_t dst_height = src_mode.vdisplay;
333 if (src_mode.interlace())
334 dst_height /= 2;
335
336 printf("src %s, crtc %s\n", src_conn->fullname().c_str(), src_mode.to_string_short().c_str());
337
338 printf("dst %s, crtc %s\n", dst_conn->fullname().c_str(), dst_mode.to_string_short().c_str());
339
340 printf("src_fb %ux%u, dst_fb %ux%u\n", src_width, src_height, dst_width, dst_height);
341
342 for (int i = 0; i < CAMERA_BUF_QUEUE_SIZE; ++i) {
343 auto fb = new DumbFramebuffer(card, dst_width, dst_height, pixfmt);
344 s_fbs.push_back(fb);
345 s_free_fbs.push_back(fb);
346 }
347
348 // get one fb for initial setup
349 s_ready_fbs.push_back(s_free_fbs.back());
350 s_free_fbs.pop_back();
351
352 // This draws a moving bar to SRC display
353 BarFlipState barflipper(card, src_crtc, src_plane, src_width, src_height);
354 barflipper.start_flipping();
355
356 // This shows the captured SRC frames on DST display
357 WBFlipState wbflipper(card, dst_crtc, dst_plane);
358
359 WBStreamer wb(vid.get_capture_streamer(), src_crtc, pixfmt);
360 wb.start_streaming();
361
362 vector<pollfd> fds(3);
363
364 fds[0].fd = 0;
365 fds[0].events = POLLIN;
366 fds[1].fd = wb.fd();
367 fds[1].events = POLLIN;
368 fds[2].fd = card.fd();
369 fds[2].events = POLLIN;
370
371 uint32_t dst_frame_num = 0;
372
373 const string filename = "wbcap.raw";
374 unique_ptr<ofstream> os;
375 if (write_file)
376 os = unique_ptr<ofstream>(new ofstream(filename, ofstream::binary));
377
378 while (true) {
379 int r = poll(fds.data(), fds.size(), -1);
380 ASSERT(r > 0);
381
382 if (fds[0].revents != 0)
383 break;
384
385 if (fds[1].revents) {
386 fds[1].revents = 0;
387
388 DumbFramebuffer* fb = wb.Dequeue();
389
390 if (write_file) {
391 printf("Writing frame %u to %s\n", dst_frame_num, filename.c_str());
392
393 for (unsigned i = 0; i < fb->num_planes(); ++i)
394 os->write((char*)fb->map(i), fb->size(i));
395
396 dst_frame_num++;
397 }
398
399 wbflipper.queue_next();
400 }
401
402 if (fds[2].revents) {
403 fds[2].revents = 0;
404
405 card.call_page_flip_handlers();
406 wb.Queue();
407 }
408 }
409
410 printf("exiting...\n");
411 }
412