Upgrade to newer bmusb, with namespacing.

[nageru] / fake_capture.cpp

diff --git a/fake_capture.cpp b/fake_capture.cpp
index 479dc6e0fa387744c4cdf300523baa1fa7afeb1d..7d23d8cafdc70df47f123f06200b9730c5897742 100644 (file)
--- a/fake_capture.cpp
+++ b/fake_capture.cpp
@@ -8,7 +8,11 @@
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
+#include <math.h>
 #include <unistd.h>
+#if __SSE2__
+#include <immintrin.h>
+#endif
 #include <cstddef>
 
 #include "bmusb/bmusb.h"
@@ -25,13 +29,28 @@ constexpr uint8_t crs[NUM_COLORS] = { 240, 34, 110, 128 };
 
 using namespace std;
 
+namespace bmusb {
 namespace {
 
-// TODO: SSE2-optimize (or at least write full int64s) if speed becomes a problem.
-
 void memset2(uint8_t *s, const uint8_t c[2], size_t n)
 {
-       for (size_t i = 0; i < n; ++i) {
+       size_t i = 0;
+#if __SSE2__
+       const uint8_t c_expanded[16] = {
+               c[0], c[1], c[0], c[1], c[0], c[1], c[0], c[1],
+               c[0], c[1], c[0], c[1], c[0], c[1], c[0], c[1]
+       };
+       __m128i cc = *(__m128i *)c_expanded;
+       __m128i *out = (__m128i *)s;
+
+       for ( ; i < (n & ~15); i += 16) {
+               _mm_storeu_si128(out++, cc);
+               _mm_storeu_si128(out++, cc);
+       }
+
+       s = (uint8_t *)out;
+#endif
+       for ( ; i < n; ++i) {
                *s++ = c[0];
                *s++ = c[1];
        }
@@ -39,7 +58,23 @@ void memset2(uint8_t *s, const uint8_t c[2], size_t n)
 
 void memset4(uint8_t *s, const uint8_t c[4], size_t n)
 {
-       for (size_t i = 0; i < n; ++i) {
+       size_t i = 0;
+#if __SSE2__
+       const uint8_t c_expanded[16] = {
+               c[0], c[1], c[2], c[3], c[0], c[1], c[2], c[3],
+               c[0], c[1], c[2], c[3], c[0], c[1], c[2], c[3]
+       };
+       __m128i cc = *(__m128i *)c_expanded;
+       __m128i *out = (__m128i *)s;
+
+       for ( ; i < (n & ~7); i += 8) {
+               _mm_storeu_si128(out++, cc);
+               _mm_storeu_si128(out++, cc);
+       }
+
+       s = (uint8_t *)out;
+#endif
+       for ( ; i < n; ++i) {
                *s++ = c[0];
                *s++ = c[1];
                *s++ = c[2];
@@ -134,6 +169,22 @@ void FakeCapture::set_audio_input(uint32_t audio_input_id)
        assert(audio_input_id == 0);
 }
 
+namespace {
+
+void add_time(double t, timespec *ts)
+{
+       ts->tv_nsec += lrint(t * 1e9);
+       ts->tv_sec += ts->tv_nsec / 1000000000;
+       ts->tv_nsec %= 1000000000;
+}
+
+bool timespec_less_than(const timespec &a, const timespec &b)
+{
+       return make_pair(a.tv_sec, a.tv_nsec) < make_pair(b.tv_sec, b.tv_nsec);
+}
+
+}  // namespace
+
 void FakeCapture::producer_thread_func()
 {
        uint16_t timecode = 0;
@@ -141,10 +192,36 @@ void FakeCapture::producer_thread_func()
        if (has_dequeue_callbacks) {
                dequeue_init_callback();
        }
+
+       timespec next_frame;
+       clock_gettime(CLOCK_MONOTONIC, &next_frame);
+       add_time(1.0 / FAKE_FPS, &next_frame);
+
        while (!producer_thread_should_quit) {
-               usleep(1000000 / FAKE_FPS);  // Rather approximate frame rate.
+               timespec now;
+               clock_gettime(CLOCK_MONOTONIC, &now);
+
+               if (timespec_less_than(now, next_frame)) {
+                       // Wait until the next frame.
+                       if (clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
+                                            &next_frame, nullptr) == -1) {
+                               if (errno == EINTR) continue;  // Re-check the flag and then sleep again.
+                               perror("clock_nanosleep");
+                               exit(1);
+                       }
+               } else {
+                       // We've seemingly missed a frame. If we're more than one second behind,
+                       // reset the timer; otherwise, just keep going.
+                       timespec limit = next_frame;
+                       ++limit.tv_sec;
+                       if (!timespec_less_than(now, limit)) {
+                               fprintf(stderr, "More than one second of missed fake frames; resetting clock.\n");
+                               next_frame = now;
+                       }
+               }
 
-               if (producer_thread_should_quit) break;
+               // Figure out when the next frame is to be, then compute the current one.
+               add_time(1.0 / FAKE_FPS, &next_frame);
 
                VideoFormat video_format;
                video_format.width = WIDTH;
@@ -152,6 +229,7 @@ void FakeCapture::producer_thread_func()
                video_format.frame_rate_nom = FAKE_FPS;
                video_format.frame_rate_den = 1;
                video_format.has_signal = true;
+               video_format.is_connected = false;
 
                FrameAllocator::Frame video_frame = video_frame_allocator->alloc_frame();
                if (video_frame.data != nullptr) {
@@ -186,3 +264,5 @@ void FakeCapture::producer_thread_func()
                dequeue_cleanup_callback();
        }
 }
+
+}  // namespace bmusb