git.sesse.net Git - bmusb/blob - bmusb.cpp

   1 // Intensity Shuttle USB3 prototype capture driver, v0.3
   2 // Can download 8-bit and 10-bit UYVY/v210 frames from HDMI, quite stable
   3 // (can do captures for hours at a time with no drops), except during startup
   4 // 576p60/720p60/1080i60 works, 1080p60 does not work (firmware limitation)
   5 // Audio comes out as 8-channel 24-bit raw audio.
   6
   7 #include <assert.h>
   8 #include <errno.h>
   9 #include <libusb.h>
  10 #include <netinet/in.h>
  11 #include <sched.h>
  12 #include <stdint.h>
  13 #include <stdio.h>
  14 #include <stdlib.h>
  15 #include <string.h>
  16 #ifdef __SSE4_1__
  17 #include <immintrin.h>
  18 #endif
  19 #include "bmusb.h"
  20
  21 #include <algorithm>
  22 #include <atomic>
  23 #include <condition_variable>
  24 #include <cstddef>
  25 #include <cstdint>
  26 #include <deque>
  27 #include <functional>
  28 #include <memory>
  29 #include <mutex>
  30 #include <stack>
  31 #include <string>
  32 #include <thread>
  33
  34 using namespace std;
  35 using namespace std::placeholders;
  36
  37 #define MIN_WIDTH 640
  38 #define HEADER_SIZE 44
  39 //#define HEADER_SIZE 0
  40 #define AUDIO_HEADER_SIZE 4
  41
  42 #define FRAME_SIZE (8 << 20)  // 8 MB.
  43 #define USB_VIDEO_TRANSFER_SIZE (128 << 10)  // 128 kB.
  44
  45 namespace {
  46
  47 FILE *audiofp;
  48
  49 thread usb_thread;
  50 atomic<bool> should_quit;
  51
  52 int find_xfer_size_for_width(int width)
  53 {
  54         // Video seems to require isochronous packets scaled with the width;
  55         // seemingly six lines is about right, rounded up to the required 1kB
  56         // multiple.
  57         int size = width * 2 * 6;
  58         // Note that for 10-bit input, you'll need to increase size accordingly.
  59         //size = size * 4 / 3;
  60         if (size % 1024 != 0) {
  61                 size &= ~1023;
  62                 size += 1024;
  63         }
  64         return size;
  65 }
  66
  67 void change_xfer_size_for_width(int width, libusb_transfer *xfr)
  68 {
  69         assert(width >= MIN_WIDTH);
  70         size_t size = find_xfer_size_for_width(width);
  71         int num_iso_pack = xfr->length / size;
  72         if (num_iso_pack != xfr->num_iso_packets ||
  73             size != xfr->iso_packet_desc[0].length) {
  74                 xfr->num_iso_packets = num_iso_pack;
  75                 libusb_set_iso_packet_lengths(xfr, size);
  76         }
  77 }
  78
  79 }  // namespace
  80
  81 FrameAllocator::~FrameAllocator() {}
  82
  83 // Audio is more important than video, and also much cheaper.
  84 // By having many more audio frames available, hopefully if something
  85 // starts to drop, we'll have CPU load go down (from not having to
  86 // process as much video) before we have to drop audio.
  87 #define NUM_QUEUED_VIDEO_FRAMES 16
  88 #define NUM_QUEUED_AUDIO_FRAMES 64
  89
  90 class MallocFrameAllocator : public FrameAllocator {
  91 public:
  92         MallocFrameAllocator(size_t frame_size, size_t num_queued_frames);
  93         Frame alloc_frame() override;
  94         void release_frame(Frame frame) override;
  95
  96 private:
  97         size_t frame_size;
  98
  99         mutex freelist_mutex;
 100         stack<unique_ptr<uint8_t[]>> freelist;  // All of size <frame_size>.
 101 };
 102
 103 MallocFrameAllocator::MallocFrameAllocator(size_t frame_size, size_t num_queued_frames)
 104         : frame_size(frame_size)
 105 {
 106         for (size_t i = 0; i < num_queued_frames; ++i) {
 107                 freelist.push(unique_ptr<uint8_t[]>(new uint8_t[frame_size]));
 108         }
 109 }
 110
 111 FrameAllocator::Frame MallocFrameAllocator::alloc_frame()
 112 {
 113         Frame vf;
 114         vf.owner = this;
 115
 116         unique_lock<mutex> lock(freelist_mutex);  // Meh.
 117         if (freelist.empty()) {
 118                 printf("Frame overrun (no more spare frames of size %ld), dropping frame!\n",
 119                         frame_size);
 120         } else {
 121                 vf.data = freelist.top().release();
 122                 vf.size = frame_size;
 123                 freelist.pop();  // Meh.
 124         }
 125         return vf;
 126 }
 127
 128 void MallocFrameAllocator::release_frame(Frame frame)
 129 {
 130         if (frame.overflow > 0) {
 131                 printf("%d bytes overflow after last (malloc) frame\n", int(frame.overflow));
 132         }
 133         unique_lock<mutex> lock(freelist_mutex);
 134         freelist.push(unique_ptr<uint8_t[]>(frame.data));
 135 }
 136
 137 bool uint16_less_than_with_wraparound(uint16_t a, uint16_t b)
 138 {
 139         if (a == b) {
 140                 return false;
 141         } else if (a < b) {
 142                 return (b - a < 0x8000);
 143         } else {
 144                 int wrap_b = 0x10000 + int(b);
 145                 return (wrap_b - a < 0x8000);
 146         }
 147 }
 148
 149 void BMUSBCapture::queue_frame(uint16_t format, uint16_t timecode, FrameAllocator::Frame frame, deque<QueuedFrame> *q)
 150 {
 151         unique_lock<mutex> lock(queue_lock);
 152         if (!q->empty() && !uint16_less_than_with_wraparound(q->back().timecode, timecode)) {
 153                 printf("Blocks going backwards: prev=0x%04x, cur=0x%04x (dropped)\n",
 154                         q->back().timecode, timecode);
 155                 frame.owner->release_frame(frame);
 156                 return;
 157         }
 158
 159         QueuedFrame qf;
 160         qf.format = format;
 161         qf.timecode = timecode;
 162         qf.frame = frame;
 163         q->push_back(move(qf));
 164         queues_not_empty.notify_one();  // might be spurious
 165 }
 166
 167 void dump_frame(const char *filename, uint8_t *frame_start, size_t frame_len)
 168 {
 169         FILE *fp = fopen(filename, "wb");
 170         if (fwrite(frame_start + HEADER_SIZE, frame_len - HEADER_SIZE, 1, fp) != 1) {
 171                 printf("short write!\n");
 172         }
 173         fclose(fp);
 174 }
 175
 176 void dump_audio_block(uint8_t *audio_start, size_t audio_len)
 177 {
 178         fwrite(audio_start + AUDIO_HEADER_SIZE, 1, audio_len - AUDIO_HEADER_SIZE, audiofp);
 179 }
 180
 181 void BMUSBCapture::dequeue_thread_func()
 182 {
 183         if (has_dequeue_callbacks) {
 184                 dequeue_init_callback();
 185         }
 186         while (!dequeue_thread_should_quit) {
 187                 unique_lock<mutex> lock(queue_lock);
 188                 queues_not_empty.wait(lock, [this]{ return dequeue_thread_should_quit || (!pending_video_frames.empty() && !pending_audio_frames.empty()); });
 189
 190                 if (dequeue_thread_should_quit) break;
 191
 192                 uint16_t video_timecode = pending_video_frames.front().timecode;
 193                 uint16_t audio_timecode = pending_audio_frames.front().timecode;
 194                 if (uint16_less_than_with_wraparound(video_timecode, audio_timecode)) {
 195                         printf("Video block 0x%04x without corresponding audio block, dropping.\n",
 196                                 video_timecode);
 197                         QueuedFrame video_frame = pending_video_frames.front();
 198                         pending_video_frames.pop_front();
 199                         lock.unlock();
 200                         video_frame_allocator->release_frame(video_frame.frame);
 201                 } else if (uint16_less_than_with_wraparound(audio_timecode, video_timecode)) {
 202                         printf("Audio block 0x%04x without corresponding video block, sending blank frame.\n",
 203                                 audio_timecode);
 204                         QueuedFrame audio_frame = pending_audio_frames.front();
 205                         pending_audio_frames.pop_front();
 206                         lock.unlock();
 207                         frame_callback(audio_timecode,
 208                                        FrameAllocator::Frame(), 0, VideoFormat(),
 209                                        audio_frame.frame, AUDIO_HEADER_SIZE, audio_frame.format);
 210                 } else {
 211                         QueuedFrame video_frame = pending_video_frames.front();
 212                         QueuedFrame audio_frame = pending_audio_frames.front();
 213                         pending_audio_frames.pop_front();
 214                         pending_video_frames.pop_front();
 215                         lock.unlock();
 216
 217 #if 0
 218                         char filename[255];
 219                         snprintf(filename, sizeof(filename), "%04x%04x.uyvy", video_frame.format, video_timecode);
 220                         dump_frame(filename, video_frame.frame.data, video_frame.data_len);
 221                         dump_audio_block(audio_frame.frame.data, audio_frame.data_len);
 222 #endif
 223
 224                         VideoFormat video_format;
 225                         if (decode_video_format(video_frame.format, &video_format)) {
 226                                 frame_callback(video_timecode,
 227                                                video_frame.frame, HEADER_SIZE, video_format,
 228                                                audio_frame.frame, AUDIO_HEADER_SIZE, audio_frame.format);
 229                         } else {
 230                                 frame_callback(video_timecode,
 231                                                FrameAllocator::Frame(), 0, video_format,
 232                                                audio_frame.frame, AUDIO_HEADER_SIZE, audio_frame.format);
 233                         }
 234                 }
 235         }
 236         if (has_dequeue_callbacks) {
 237                 dequeue_cleanup_callback();
 238         }
 239 }
 240
 241 void BMUSBCapture::start_new_frame(const uint8_t *start)
 242 {
 243         uint16_t format = (start[3] << 8) | start[2];
 244         uint16_t timecode = (start[1] << 8) | start[0];
 245
 246         if (current_video_frame.len > 0) {
 247                 // If format is 0x0800 (no signal), add a fake (empty) audio
 248                 // frame to get it out of the queue.
 249                 // TODO: Figure out if there are other formats that come with
 250                 // no audio, and treat them the same.
 251                 if (format == 0x0800) {
 252                         FrameAllocator::Frame fake_audio_frame = audio_frame_allocator->alloc_frame();
 253                         if (fake_audio_frame.data == nullptr) {
 254                                 // Oh well, it's just a no-signal frame anyway.
 255                                 printf("Couldn't allocate fake audio frame, also dropping no-signal video frame.\n");
 256                                 current_video_frame.owner->release_frame(current_video_frame);
 257                                 current_video_frame = video_frame_allocator->alloc_frame();
 258                                 return;
 259                         }
 260                         queue_frame(format, timecode, fake_audio_frame, &pending_audio_frames);
 261                 }
 262                 //dump_frame();
 263                 queue_frame(format, timecode, current_video_frame, &pending_video_frames);
 264
 265                 // Update the assumed frame width. We might be one frame too late on format changes,
 266                 // but it's much better than asking the user to choose manually.
 267                 VideoFormat video_format;
 268                 if (decode_video_format(format, &video_format)) {
 269                         assumed_frame_width = video_format.width;
 270                 }
 271         }
 272         //printf("Found frame start, format 0x%04x timecode 0x%04x, previous frame length was %d/%d\n",
 273         //      format, timecode,
 274         //      //start[7], start[6], start[5], start[4],
 275         //      read_current_frame, FRAME_SIZE);
 276
 277         current_video_frame = video_frame_allocator->alloc_frame();
 278         //if (current_video_frame.data == nullptr) {
 279         //      read_current_frame = -1;
 280         //} else {
 281         //      read_current_frame = 0;
 282         //}
 283 }
 284
 285 void BMUSBCapture::start_new_audio_block(const uint8_t *start)
 286 {
 287         uint16_t format = (start[3] << 8) | start[2];
 288         uint16_t timecode = (start[1] << 8) | start[0];
 289         if (current_audio_frame.len > 0) {
 290                 //dump_audio_block();
 291                 queue_frame(format, timecode, current_audio_frame, &pending_audio_frames);
 292         }
 293         //printf("Found audio block start, format 0x%04x timecode 0x%04x, previous block length was %d\n",
 294         //      format, timecode, read_current_audio_block);
 295         current_audio_frame = audio_frame_allocator->alloc_frame();
 296 }
 297
 298 #if 0
 299 static void dump_pack(const libusb_transfer *xfr, int offset, const libusb_iso_packet_descriptor *pack)
 300 {
 301         //      printf("ISO pack%u length:%u, actual_length:%u, offset:%u\n", i, pack->length, pack->actual_length, offset);
 302         for (unsigned j = 0; j < pack->actual_length; j++) {
 303         //for (int j = 0; j < min(pack->actual_length, 16u); j++) {
 304                 printf("%02x", xfr->buffer[j + offset]);
 305                 if ((j % 16) == 15)
 306                         printf("\n");
 307                 else if ((j % 8) == 7)
 308                         printf("  ");
 309                 else
 310                         printf(" ");
 311         }
 312 }
 313 #endif
 314
 315 void memcpy_interleaved(uint8_t *dest1, uint8_t *dest2, const uint8_t *src, size_t n)
 316 {
 317         assert(n % 2 == 0);
 318         uint8_t *dptr1 = dest1;
 319         uint8_t *dptr2 = dest2;
 320
 321         for (size_t i = 0; i < n; i += 2) {
 322                 *dptr1++ = *src++;
 323                 *dptr2++ = *src++;
 324         }
 325 }
 326
 327 void add_to_frame(FrameAllocator::Frame *current_frame, const char *frame_type_name, const uint8_t *start, const uint8_t *end)
 328 {
 329         if (current_frame->data == nullptr ||
 330             current_frame->len > current_frame->size ||
 331             start == end) {
 332                 return;
 333         }
 334
 335         int bytes = end - start;
 336         if (current_frame->len + bytes > current_frame->size) {
 337                 current_frame->overflow = current_frame->len + bytes - current_frame->size;
 338                 current_frame->len = current_frame->size;
 339                 if (current_frame->overflow > 1048576) {
 340                         printf("%d bytes overflow after last %s frame\n",
 341                                 int(current_frame->overflow), frame_type_name);
 342                         current_frame->overflow = 0;
 343                 }
 344                 //dump_frame();
 345         } else {
 346                 if (current_frame->interleaved) {
 347                         uint8_t *data = current_frame->data + current_frame->len / 2;
 348                         uint8_t *data2 = current_frame->data2 + current_frame->len / 2;
 349                         if (current_frame->len % 2 == 1) {
 350                                 ++data;
 351                                 swap(data, data2);
 352                         }
 353                         if (bytes % 2 == 1) {
 354                                 *data++ = *start++;
 355                                 swap(data, data2);
 356                                 ++current_frame->len;
 357                                 --bytes;
 358                         }
 359                         memcpy_interleaved(data, data2, start, bytes);
 360                         current_frame->len += bytes;
 361                 } else {
 362                         memcpy(current_frame->data + current_frame->len, start, bytes);
 363                         current_frame->len += bytes;
 364                 }
 365         }
 366 }
 367
 368 #ifdef __SSE4_1__
 369
 370 #if 0
 371 void avx2_dump(const char *name, __m256i n)
 372 {
 373         printf("%-10s:", name);
 374         printf(" %02x", _mm256_extract_epi8(n, 0));
 375         printf(" %02x", _mm256_extract_epi8(n, 1));
 376         printf(" %02x", _mm256_extract_epi8(n, 2));
 377         printf(" %02x", _mm256_extract_epi8(n, 3));
 378         printf(" %02x", _mm256_extract_epi8(n, 4));
 379         printf(" %02x", _mm256_extract_epi8(n, 5));
 380         printf(" %02x", _mm256_extract_epi8(n, 6));
 381         printf(" %02x", _mm256_extract_epi8(n, 7));
 382         printf(" ");
 383         printf(" %02x", _mm256_extract_epi8(n, 8));
 384         printf(" %02x", _mm256_extract_epi8(n, 9));
 385         printf(" %02x", _mm256_extract_epi8(n, 10));
 386         printf(" %02x", _mm256_extract_epi8(n, 11));
 387         printf(" %02x", _mm256_extract_epi8(n, 12));
 388         printf(" %02x", _mm256_extract_epi8(n, 13));
 389         printf(" %02x", _mm256_extract_epi8(n, 14));
 390         printf(" %02x", _mm256_extract_epi8(n, 15));
 391         printf(" ");
 392         printf(" %02x", _mm256_extract_epi8(n, 16));
 393         printf(" %02x", _mm256_extract_epi8(n, 17));
 394         printf(" %02x", _mm256_extract_epi8(n, 18));
 395         printf(" %02x", _mm256_extract_epi8(n, 19));
 396         printf(" %02x", _mm256_extract_epi8(n, 20));
 397         printf(" %02x", _mm256_extract_epi8(n, 21));
 398         printf(" %02x", _mm256_extract_epi8(n, 22));
 399         printf(" %02x", _mm256_extract_epi8(n, 23));
 400         printf(" ");
 401         printf(" %02x", _mm256_extract_epi8(n, 24));
 402         printf(" %02x", _mm256_extract_epi8(n, 25));
 403         printf(" %02x", _mm256_extract_epi8(n, 26));
 404         printf(" %02x", _mm256_extract_epi8(n, 27));
 405         printf(" %02x", _mm256_extract_epi8(n, 28));
 406         printf(" %02x", _mm256_extract_epi8(n, 29));
 407         printf(" %02x", _mm256_extract_epi8(n, 30));
 408         printf(" %02x", _mm256_extract_epi8(n, 31));
 409         printf("\n");
 410 }
 411 #endif
 412
 413 // Does a memcpy and memchr in one to reduce processing time.
 414 // Note that the benefit is somewhat limited if your L3 cache is small,
 415 // as you'll (unfortunately) spend most of the time loading the data
 416 // from main memory.
 417 //
 418 // Complicated cases are left to the slow path; it basically stops copying
 419 // up until the first instance of "sync_char" (usually a bit before, actually).
 420 // This is fine, since 0x00 bytes shouldn't really show up in normal picture
 421 // data, and what we really need this for is the 00 00 ff ff marker in video data.
 422 const uint8_t *add_to_frame_fastpath(FrameAllocator::Frame *current_frame, const uint8_t *start, const uint8_t *limit, const char sync_char)
 423 {
 424         if (current_frame->data == nullptr ||
 425             current_frame->len > current_frame->size ||
 426             start == limit) {
 427                 return start;
 428         }
 429         size_t orig_bytes = limit - start;
 430         if (orig_bytes < 128) {
 431                 // Don't bother.
 432                 return start;
 433         }
 434
 435         // Don't read more bytes than we can write.
 436         limit = min(limit, start + (current_frame->size - current_frame->len));
 437
 438         // Align end to 32 bytes.
 439         limit = (const uint8_t *)(intptr_t(limit) & ~31);
 440
 441         if (start >= limit) {
 442                 return start;
 443         }
 444
 445         // Process [0,31] bytes, such that start gets aligned to 32 bytes.
 446         const uint8_t *aligned_start = (const uint8_t *)(intptr_t(start + 31) & ~31);
 447         if (aligned_start != start) {
 448                 const uint8_t *sync_start = (const uint8_t *)memchr(start, sync_char, aligned_start - start);
 449                 if (sync_start == nullptr) {
 450                         add_to_frame(current_frame, "", start, aligned_start);
 451                 } else {
 452                         add_to_frame(current_frame, "", start, sync_start);
 453                         return sync_start;
 454                 }
 455         }
 456
 457         // Make the length a multiple of 64.
 458         if (current_frame->interleaved) {
 459                 if (((limit - aligned_start) % 64) != 0) {
 460                         limit -= 32;
 461                 }
 462                 assert(((limit - aligned_start) % 64) == 0);
 463         }
 464
 465 #if __AVX2__
 466         const __m256i needle = _mm256_set1_epi8(sync_char);
 467
 468         const __restrict __m256i *in = (const __m256i *)aligned_start;
 469         if (current_frame->interleaved) {
 470                 __restrict __m256i *out1 = (__m256i *)(current_frame->data + (current_frame->len + 1) / 2);
 471                 __restrict __m256i *out2 = (__m256i *)(current_frame->data2 + current_frame->len / 2);
 472                 if (current_frame->len % 2 == 1) {
 473                         swap(out1, out2);
 474                 }
 475
 476                 __m256i shuffle_cw = _mm256_set_epi8(
 477                         15, 13, 11, 9, 7, 5, 3, 1, 14, 12, 10, 8, 6, 4, 2, 0,
 478                         15, 13, 11, 9, 7, 5, 3, 1, 14, 12, 10, 8, 6, 4, 2, 0);
 479                 while (in < (const __m256i *)limit) {
 480                         // Note: For brevity, comments show lanes as if they were 2x64-bit (they're actually 2x128).
 481                         __m256i data1 = _mm256_stream_load_si256(in);         // AaBbCcDd EeFfGgHh
 482                         __m256i data2 = _mm256_stream_load_si256(in + 1);     // IiJjKkLl MmNnOoPp
 483
 484                         __m256i found1 = _mm256_cmpeq_epi8(data1, needle);
 485                         __m256i found2 = _mm256_cmpeq_epi8(data2, needle);
 486                         __m256i found = _mm256_or_si256(found1, found2);
 487
 488                         data1 = _mm256_shuffle_epi8(data1, shuffle_cw);       // ABCDabcd EFGHefgh
 489                         data2 = _mm256_shuffle_epi8(data2, shuffle_cw);       // IJKLijkl MNOPmnop
 490
 491                         data1 = _mm256_permute4x64_epi64(data1, 0b11011000);  // ABCDEFGH abcdefgh
 492                         data2 = _mm256_permute4x64_epi64(data2, 0b11011000);  // IJKLMNOP ijklmnop
 493
 494                         __m256i lo = _mm256_permute2x128_si256(data1, data2, 0b00100000);
 495                         __m256i hi = _mm256_permute2x128_si256(data1, data2, 0b00110001);
 496
 497                         _mm256_storeu_si256(out1, lo);  // Store as early as possible, even if the data isn't used.
 498                         _mm256_storeu_si256(out2, hi);
 499
 500                         if (!_mm256_testz_si256(found, found)) {
 501                                 break;
 502                         }
 503
 504                         in += 2;
 505                         ++out1;
 506                         ++out2;
 507                 }
 508                 current_frame->len += (uint8_t *)in - aligned_start;
 509         } else {
 510                 __m256i *out = (__m256i *)(current_frame->data + current_frame->len);
 511                 while (in < (const __m256i *)limit) {
 512                         __m256i data = _mm256_load_si256(in);
 513                         _mm256_storeu_si256(out, data);  // Store as early as possible, even if the data isn't used.
 514                         __m256i found = _mm256_cmpeq_epi8(data, needle);
 515                         if (!_mm256_testz_si256(found, found)) {
 516                                 break;
 517                         }
 518
 519                         ++in;
 520                         ++out;
 521                 }
 522                 current_frame->len = (uint8_t *)out - current_frame->data;
 523         }
 524 #else
 525         const __m128i needle = _mm_set1_epi8(sync_char);
 526
 527         const __m128i *in = (const __m128i *)aligned_start;
 528         if (current_frame->interleaved) {
 529                 __m128i *out1 = (__m128i *)(current_frame->data + (current_frame->len + 1) / 2);
 530                 __m128i *out2 = (__m128i *)(current_frame->data2 + current_frame->len / 2);
 531                 if (current_frame->len % 2 == 1) {
 532                         swap(out1, out2);
 533                 }
 534
 535                 __m128i mask_lower_byte = _mm_set1_epi16(0x00ff);
 536                 while (in < (const __m128i *)limit) {
 537                         __m128i data1 = _mm_load_si128(in);
 538                         __m128i data2 = _mm_load_si128(in + 1);
 539                         __m128i data1_lo = _mm_and_si128(data1, mask_lower_byte);
 540                         __m128i data2_lo = _mm_and_si128(data2, mask_lower_byte);
 541                         __m128i data1_hi = _mm_srli_epi16(data1, 8);
 542                         __m128i data2_hi = _mm_srli_epi16(data2, 8);
 543                         __m128i lo = _mm_packus_epi16(data1_lo, data2_lo);
 544                         _mm_storeu_si128(out1, lo);  // Store as early as possible, even if the data isn't used.
 545                         __m128i hi = _mm_packus_epi16(data1_hi, data2_hi);
 546                         _mm_storeu_si128(out2, hi);
 547                         __m128i found1 = _mm_cmpeq_epi8(data1, needle);
 548                         __m128i found2 = _mm_cmpeq_epi8(data2, needle);
 549                         if (!_mm_testz_si128(found1, found1) ||
 550                             !_mm_testz_si128(found2, found2)) {
 551                                 break;
 552                         }
 553
 554                         in += 2;
 555                         ++out1;
 556                         ++out2;
 557                 }
 558                 current_frame->len += (uint8_t *)in - aligned_start;
 559         } else {
 560                 __m128i *out = (__m128i *)(current_frame->data + current_frame->len);
 561                 while (in < (const __m128i *)limit) {
 562                         __m128i data = _mm_load_si128(in);
 563                         _mm_storeu_si128(out, data);  // Store as early as possible, even if the data isn't used.
 564                         __m128i found = _mm_cmpeq_epi8(data, needle);
 565                         if (!_mm_testz_si128(found, found)) {
 566                                 break;
 567                         }
 568
 569                         ++in;
 570                         ++out;
 571                 }
 572                 current_frame->len = (uint8_t *)out - current_frame->data;
 573         }
 574 #endif
 575
 576         //printf("managed to fastpath %ld/%ld bytes\n", (const uint8_t *)in - (const uint8_t *)aligned_start, orig_bytes);
 577
 578         return (const uint8_t *)in;
 579 }
 580 #endif
 581
 582 void decode_packs(const libusb_transfer *xfr,
 583                   const char *sync_pattern,
 584                   int sync_length,
 585                   FrameAllocator::Frame *current_frame,
 586                   const char *frame_type_name,
 587                   function<void(const uint8_t *start)> start_callback)
 588 {
 589         int offset = 0;
 590         for (int i = 0; i < xfr->num_iso_packets; i++) {
 591                 const libusb_iso_packet_descriptor *pack = &xfr->iso_packet_desc[i];
 592
 593                 if (pack->status != LIBUSB_TRANSFER_COMPLETED) {
 594                         fprintf(stderr, "Error: pack %u/%u status %d\n", i, xfr->num_iso_packets, pack->status);
 595                         continue;
 596 //exit(5);
 597                 }
 598
 599                 const uint8_t *start = xfr->buffer + offset;
 600                 const uint8_t *limit = start + pack->actual_length;
 601                 while (start < limit) {  // Usually runs only one iteration.
 602 #ifdef __SSE4_1__
 603                         start = add_to_frame_fastpath(current_frame, start, limit, sync_pattern[0]);
 604                         if (start == limit) break;
 605                         assert(start < limit);
 606 #endif
 607
 608                         const unsigned char* start_next_frame = (const unsigned char *)memmem(start, limit - start, sync_pattern, sync_length);
 609                         if (start_next_frame == nullptr) {
 610                                 // add the rest of the buffer
 611                                 add_to_frame(current_frame, frame_type_name, start, limit);
 612                                 break;
 613                         } else {
 614                                 add_to_frame(current_frame, frame_type_name, start, start_next_frame);
 615                                 start = start_next_frame + sync_length;  // skip sync
 616                                 start_callback(start);
 617                         }
 618                 }
 619 #if 0
 620                 dump_pack(xfr, offset, pack);
 621 #endif
 622                 offset += pack->length;
 623         }
 624 }
 625
 626 void BMUSBCapture::cb_xfr(struct libusb_transfer *xfr)
 627 {
 628         if (xfr->status != LIBUSB_TRANSFER_COMPLETED) {
 629                 fprintf(stderr, "transfer status %d\n", xfr->status);
 630                 libusb_free_transfer(xfr);
 631                 exit(3);
 632         }
 633
 634         assert(xfr->user_data != nullptr);
 635         BMUSBCapture *usb = static_cast<BMUSBCapture *>(xfr->user_data);
 636
 637         if (xfr->type == LIBUSB_TRANSFER_TYPE_ISOCHRONOUS) {
 638                 if (xfr->endpoint == 0x84) {
 639                         decode_packs(xfr, "DeckLinkAudioResyncT", 20, &usb->current_audio_frame, "audio", bind(&BMUSBCapture::start_new_audio_block, usb, _1));
 640                 } else {
 641                         decode_packs(xfr, "\x00\x00\xff\xff", 4, &usb->current_video_frame, "video", bind(&BMUSBCapture::start_new_frame, usb, _1));
 642
 643                         // Update the transfer with the new assumed width, if we're in the process of changing formats.
 644                         change_xfer_size_for_width(usb->assumed_frame_width, xfr);
 645                 }
 646         }
 647         if (xfr->type == LIBUSB_TRANSFER_TYPE_CONTROL) {
 648                 //const libusb_control_setup *setup = libusb_control_transfer_get_setup(xfr);
 649                 uint8_t *buf = libusb_control_transfer_get_data(xfr);
 650 #if 0
 651                 if (setup->wIndex == 44) {
 652                         printf("read timer register: 0x%02x%02x%02x%02x\n", buf[0], buf[1], buf[2], buf[3]);
 653                 } else {
 654                         printf("read register %2d:                      0x%02x%02x%02x%02x\n",
 655                                 setup->wIndex, buf[0], buf[1], buf[2], buf[3]);
 656                 }
 657 #else
 658                 memcpy(usb->register_file + usb->current_register, buf, 4);
 659                 usb->current_register = (usb->current_register + 4) % NUM_BMUSB_REGISTERS;
 660                 if (usb->current_register == 0) {
 661                         // read through all of them
 662                         printf("register dump:");
 663                         for (int i = 0; i < NUM_BMUSB_REGISTERS; i += 4) {
 664                                 printf(" 0x%02x%02x%02x%02x", usb->register_file[i], usb->register_file[i + 1], usb->register_file[i + 2], usb->register_file[i + 3]);
 665                         }
 666                         printf("\n");
 667                 }
 668                 libusb_fill_control_setup(xfr->buffer,
 669                     LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_IN, /*request=*/214, /*value=*/0,
 670                         /*index=*/usb->current_register, /*length=*/4);
 671 #endif
 672         }
 673
 674 #if 0
 675         printf("length:%u, actual_length:%u\n", xfr->length, xfr->actual_length);
 676         for (i = 0; i < xfr->actual_length; i++) {
 677                 printf("%02x", xfr->buffer[i]);
 678                 if (i % 16)
 679                         printf("\n");
 680                 else if (i % 8)
 681                         printf("  ");
 682                 else
 683                         printf(" ");
 684         }
 685 #endif
 686
 687         int rc = libusb_submit_transfer(xfr);
 688         if (rc < 0) {
 689                 fprintf(stderr, "error re-submitting URB: %s\n", libusb_error_name(rc));
 690                 exit(1);
 691         }
 692 }
 693
 694 void BMUSBCapture::usb_thread_func()
 695 {
 696         sched_param param;
 697         memset(&param, 0, sizeof(param));
 698         param.sched_priority = 1;
 699         if (sched_setscheduler(0, SCHED_RR, &param) == -1) {
 700                 printf("couldn't set realtime priority for USB thread: %s\n", strerror(errno));
 701         }
 702         while (!should_quit) {
 703                 int rc = libusb_handle_events(nullptr);
 704                 if (rc != LIBUSB_SUCCESS)
 705                         break;
 706         }
 707 }
 708
 709 struct USBCardDevice {
 710         uint16_t product;
 711         uint8_t bus, port;
 712         libusb_device *device;
 713 };
 714
 715 libusb_device_handle *open_card(int card_index, string *description)
 716 {
 717         libusb_device **devices;
 718         ssize_t num_devices = libusb_get_device_list(nullptr, &devices);
 719         if (num_devices == -1) {
 720                 fprintf(stderr, "Error finding USB devices\n");
 721                 exit(1);
 722         }
 723         vector<USBCardDevice> found_cards;
 724         for (ssize_t i = 0; i < num_devices; ++i) {
 725                 libusb_device_descriptor desc;
 726                 if (libusb_get_device_descriptor(devices[i], &desc) < 0) {
 727                         fprintf(stderr, "Error getting device descriptor for device %d\n", int(i));
 728                         exit(1);
 729                 }
 730
 731                 uint8_t bus = libusb_get_bus_number(devices[i]);
 732                 uint8_t port = libusb_get_port_number(devices[i]);
 733
 734                 if (!(desc.idVendor == 0x1edb && desc.idProduct == 0xbd3b) &&
 735                     !(desc.idVendor == 0x1edb && desc.idProduct == 0xbd4f)) {
 736                         libusb_unref_device(devices[i]);
 737                         continue;
 738                 }
 739
 740                 found_cards.push_back({ desc.idProduct, bus, port, devices[i] });
 741         }
 742         libusb_free_device_list(devices, 0);
 743
 744         // Sort the devices to get a consistent ordering.
 745         sort(found_cards.begin(), found_cards.end(), [](const USBCardDevice &a, const USBCardDevice &b) {
 746                 if (a.product != b.product)
 747                         return a.product < b.product;
 748                 if (a.bus != b.bus)
 749                         return a.bus < b.bus;
 750                 return a.port < b.port;
 751         });
 752
 753         for (size_t i = 0; i < found_cards.size(); ++i) {
 754                 const char *product_name = nullptr;
 755                 if (found_cards[i].product == 0xbd3b) {
 756                         product_name = "Intensity Shuttle";
 757                 } else if (found_cards[i].product == 0xbd4f) {
 758                         product_name = "UltraStudio SDI";
 759                 } else {
 760                         assert(false);
 761                 }
 762
 763                 char buf[256];
 764                 snprintf(buf, sizeof(buf), "Card %d: Bus %03u Device %03u  %s",
 765                         int(i), found_cards[i].bus, found_cards[i].port, product_name);
 766                 if (i == size_t(card_index)) {
 767                         *description = buf;
 768                 }
 769                 fprintf(stderr, "%s\n", buf);
 770         }
 771
 772         if (size_t(card_index) >= found_cards.size()) {
 773                 fprintf(stderr, "Could not open card %d (only %d found)\n", card_index, int(found_cards.size()));
 774                 exit(1);
 775         }
 776
 777         libusb_device_handle *devh;
 778         int rc = libusb_open(found_cards[card_index].device, &devh);
 779         if (rc < 0) {
 780                 fprintf(stderr, "Error opening card %d: %s\n", card_index, libusb_error_name(rc));
 781                 exit(1);
 782         }
 783
 784         for (size_t i = 0; i < found_cards.size(); ++i) {
 785                 libusb_unref_device(found_cards[i].device);
 786         }
 787
 788         return devh;
 789 }
 790
 791 void BMUSBCapture::configure_card()
 792 {
 793         if (video_frame_allocator == nullptr) {
 794                 set_video_frame_allocator(new MallocFrameAllocator(FRAME_SIZE, NUM_QUEUED_VIDEO_FRAMES));  // FIXME: leak.
 795         }
 796         if (audio_frame_allocator == nullptr) {
 797                 set_audio_frame_allocator(new MallocFrameAllocator(65536, NUM_QUEUED_AUDIO_FRAMES));  // FIXME: leak.
 798         }
 799         dequeue_thread_should_quit = false;
 800         dequeue_thread = thread(&BMUSBCapture::dequeue_thread_func, this);
 801
 802         int rc;
 803         struct libusb_transfer *xfr;
 804
 805         rc = libusb_init(nullptr);
 806         if (rc < 0) {
 807                 fprintf(stderr, "Error initializing libusb: %s\n", libusb_error_name(rc));
 808                 exit(1);
 809         }
 810
 811         libusb_device_handle *devh = open_card(card_index, &description);
 812         if (!devh) {
 813                 fprintf(stderr, "Error finding USB device\n");
 814                 exit(1);
 815         }
 816
 817         libusb_config_descriptor *config;
 818         rc = libusb_get_config_descriptor(libusb_get_device(devh), /*config_index=*/0, &config);
 819         if (rc < 0) {
 820                 fprintf(stderr, "Error getting configuration: %s\n", libusb_error_name(rc));
 821                 exit(1);
 822         }
 823
 824 #if 0
 825         printf("%d interface\n", config->bNumInterfaces);
 826         for (int interface_number = 0; interface_number < config->bNumInterfaces; ++interface_number) {
 827                 printf("  interface %d\n", interface_number);
 828                 const libusb_interface *interface = &config->interface[interface_number];
 829                 for (int altsetting = 0; altsetting < interface->num_altsetting; ++altsetting) {
 830                         const libusb_interface_descriptor *interface_desc = &interface->altsetting[altsetting];
 831                         printf("    alternate setting %d\n", interface_desc->bAlternateSetting);
 832                         for (int endpoint_number = 0; endpoint_number < interface_desc->bNumEndpoints; ++endpoint_number) {
 833                                 const libusb_endpoint_descriptor *endpoint = &interface_desc->endpoint[endpoint_number];
 834                                 printf("        endpoint address 0x%02x\n", endpoint->bEndpointAddress);
 835                         }
 836                 }
 837         }
 838 #endif
 839
 840         rc = libusb_set_configuration(devh, /*configuration=*/1);
 841         if (rc < 0) {
 842                 fprintf(stderr, "Error setting configuration 1: %s\n", libusb_error_name(rc));
 843                 exit(1);
 844         }
 845
 846         rc = libusb_claim_interface(devh, 0);
 847         if (rc < 0) {
 848                 fprintf(stderr, "Error claiming interface 0: %s\n", libusb_error_name(rc));
 849                 exit(1);
 850         }
 851
 852         // Alternate setting 1 is output, alternate setting 2 is input.
 853         // Card is reset when switching alternates, so the driver uses
 854         // this “double switch” when it wants to reset.
 855         //
 856         // There's also alternate settings 3 and 4, which seem to be
 857         // like 1 and 2 except they advertise less bandwidth needed.
 858         rc = libusb_set_interface_alt_setting(devh, /*interface=*/0, /*alternate_setting=*/1);
 859         if (rc < 0) {
 860                 fprintf(stderr, "Error setting alternate 1: %s\n", libusb_error_name(rc));
 861                 exit(1);
 862         }
 863         rc = libusb_set_interface_alt_setting(devh, /*interface=*/0, /*alternate_setting=*/2);
 864         if (rc < 0) {
 865                 fprintf(stderr, "Error setting alternate 2: %s\n", libusb_error_name(rc));
 866                 exit(1);
 867         }
 868 #if 0
 869         rc = libusb_set_interface_alt_setting(devh, /*interface=*/0, /*alternate_setting=*/1);
 870         if (rc < 0) {
 871                 fprintf(stderr, "Error setting alternate 1: %s\n", libusb_error_name(rc));
 872                 exit(1);
 873         }
 874 #endif
 875
 876 #if 0
 877         rc = libusb_claim_interface(devh, 3);
 878         if (rc < 0) {
 879                 fprintf(stderr, "Error claiming interface 3: %s\n", libusb_error_name(rc));
 880                 exit(1);
 881         }
 882 #endif
 883
 884         // theories:
 885         //   44 is some kind of timer register (first 16 bits count upwards)
 886         //   24 is some sort of watchdog?
 887         //      you can seemingly set it to 0x73c60001 and that bit will eventually disappear
 888         //      (or will go to 0x73c60010?), also seen 0x73c60100
 889         //   12 also changes all the time, unclear why
 890         //   16 seems to be autodetected mode somehow
 891         //      --    this is e00115e0 after reset?
 892         //                    ed0115e0 after mode change [to output?]
 893         //                    2d0015e0 after more mode change [to input]
 894         //                    ed0115e0 after more mode change
 895         //                    2d0015e0 after more mode change
 896         //
 897         //                    390115e0 seems to indicate we have signal
 898         //         changes to 200115e0 when resolution changes/we lose signal, driver resets after a while
 899         //
 900         //                    200015e0 on startup
 901         //         changes to 250115e0 when we sync to the signal
 902         //
 903         //    so only first 16 bits count, and 0x0100 is a mask for ok/stable signal?
 904         //
 905         //    Bottom 16 bits of this register seem to be firmware version number (possibly not all all of them).
 906         //
 907         //    28 and 32 seems to be analog audio input levels (one byte for each of the eight channels).
 908         //    however, if setting 32 with HDMI embedded audio, it is immediately overwritten back (to 0xe137002a).
 909         //
 910         //    4, 8, 20 are unclear. seem to be some sort of bitmask, but we can set them to 0 with no apparent effect.
 911         //    perhaps some of them are related to analog output?
 912         //
 913         //    36 can be set to 0 with no apparent effect (all of this tested on both video and audio),
 914         //    but the driver sets it to 0x8036802a at some point.
 915         //
 916         //    all of this is on request 214/215. other requests (192, 219,
 917         //    222, 223, 224) are used for firmware upgrade. Probably best to
 918         //    stay out of it unless you know what you're doing.
 919         //
 920         //
 921         // register 16:
 922         // first byte is 0x39 for a stable 576p60 signal, 0x2d for a stable 720p60 signal, 0x20 for no signal
 923         //
 924         // theories:
 925         //   0x01 - stable signal
 926         //   0x04 - deep color
 927         //   0x08 - unknown (audio??)
 928         //   0x20 - 720p??
 929         //   0x30 - 576p??
 930
 931         struct ctrl {
 932                 int endpoint;
 933                 int request;
 934                 int index;
 935                 uint32_t data;
 936         };
 937         static const ctrl ctrls[] = {
 938                 { LIBUSB_ENDPOINT_IN,  214, 16, 0 },
 939                 { LIBUSB_ENDPOINT_IN,  214,  0, 0 },
 940
 941                 // seems to capture on HDMI, clearing the 0x20000000 bit seems to activate 10-bit
 942                 // capture (v210).
 943                 // clearing the 0x08000000 bit seems to change the capture format (other source?)
 944                 // 0x10000000 = analog audio instead of embedded audio, it seems
 945                 // 0x3a000000 = component video? (analog audio)
 946                 // 0x3c000000 = composite video? (analog audio)
 947                 // 0x3e000000 = s-video? (analog audio)
 948                 { LIBUSB_ENDPOINT_OUT, 215,  0, 0x29000000 },
 949                 //{ LIBUSB_ENDPOINT_OUT, 215,  0, 0x80000100 },
 950                 //{ LIBUSB_ENDPOINT_OUT, 215,  0, 0x09000000 },
 951                 { LIBUSB_ENDPOINT_OUT, 215, 24, 0x73c60001 },  // latch for frame start?
 952                 { LIBUSB_ENDPOINT_IN,  214, 24, 0 },  //
 953         };
 954
 955         for (unsigned req = 0; req < sizeof(ctrls) / sizeof(ctrls[0]); ++req) {
 956                 uint32_t flipped = htonl(ctrls[req].data);
 957                 static uint8_t value[4];
 958                 memcpy(value, &flipped, sizeof(flipped));
 959                 int size = sizeof(value);
 960                 //if (ctrls[req].request == 215) size = 0;
 961                 rc = libusb_control_transfer(devh, LIBUSB_REQUEST_TYPE_VENDOR | ctrls[req].endpoint,
 962                         /*request=*/ctrls[req].request, /*value=*/0, /*index=*/ctrls[req].index, value, size, /*timeout=*/0);
 963                 if (rc < 0) {
 964                         fprintf(stderr, "Error on control %d: %s\n", ctrls[req].index, libusb_error_name(rc));
 965                         exit(1);
 966                 }
 967
 968                 if (ctrls[req].index == 16 && rc == 4) {
 969                         printf("Card firmware version: 0x%02x%02x\n", value[2], value[3]);
 970                 }
 971
 972 #if 0
 973                 printf("rc=%d: ep=%d@%d %d -> 0x", rc, ctrls[req].endpoint, ctrls[req].request, ctrls[req].index);
 974                 for (int i = 0; i < rc; ++i) {
 975                         printf("%02x", value[i]);
 976                 }
 977                 printf("\n");
 978 #endif
 979         }
 980
 981 #if 0
 982         // DEBUG
 983         for ( ;; ) {
 984                 static int my_index = 0;
 985                 static uint8_t value[4];
 986                 int size = sizeof(value);
 987                 rc = libusb_control_transfer(devh, LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_IN,
 988                         /*request=*/214, /*value=*/0, /*index=*/my_index, value, size, /*timeout=*/0);
 989                 if (rc < 0) {
 990                         fprintf(stderr, "Error on control\n");
 991                         exit(1);
 992                 }
 993                 printf("rc=%d index=%d: 0x", rc, my_index);
 994                 for (int i = 0; i < rc; ++i) {
 995                         printf("%02x", value[i]);
 996                 }
 997                 printf("\n");
 998         }
 999 #endif
1000
1001 #if 0
1002         // set up an asynchronous transfer of the timer register
1003         static uint8_t cmdbuf[LIBUSB_CONTROL_SETUP_SIZE + 4];
1004         static int completed = 0;
1005
1006         xfr = libusb_alloc_transfer(0);
1007         libusb_fill_control_setup(cmdbuf,
1008             LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_IN, /*request=*/214, /*value=*/0,
1009                 /*index=*/44, /*length=*/4);
1010         libusb_fill_control_transfer(xfr, devh, cmdbuf, cb_xfr, &completed, 0);
1011         xfr->user_data = this;
1012         libusb_submit_transfer(xfr);
1013
1014         // set up an asynchronous transfer of register 24
1015         static uint8_t cmdbuf2[LIBUSB_CONTROL_SETUP_SIZE + 4];
1016         static int completed2 = 0;
1017
1018         xfr = libusb_alloc_transfer(0);
1019         libusb_fill_control_setup(cmdbuf2,
1020             LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_IN, /*request=*/214, /*value=*/0,
1021                 /*index=*/24, /*length=*/4);
1022         libusb_fill_control_transfer(xfr, devh, cmdbuf2, cb_xfr, &completed2, 0);
1023         xfr->user_data = this;
1024         libusb_submit_transfer(xfr);
1025 #endif
1026
1027         // set up an asynchronous transfer of the register dump
1028         static uint8_t cmdbuf3[LIBUSB_CONTROL_SETUP_SIZE + 4];
1029         static int completed3 = 0;
1030
1031         xfr = libusb_alloc_transfer(0);
1032         libusb_fill_control_setup(cmdbuf3,
1033             LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_IN, /*request=*/214, /*value=*/0,
1034                 /*index=*/current_register, /*length=*/4);
1035         libusb_fill_control_transfer(xfr, devh, cmdbuf3, cb_xfr, &completed3, 0);
1036         xfr->user_data = this;
1037         //libusb_submit_transfer(xfr);
1038
1039         audiofp = fopen("audio.raw", "wb");
1040
1041         // set up isochronous transfers for audio and video
1042         for (int e = 3; e <= 4; ++e) {
1043                 //int num_transfers = (e == 3) ? 6 : 6;
1044                 int num_transfers = 10;
1045                 for (int i = 0; i < num_transfers; ++i) {
1046                         size_t buf_size;
1047                         int num_iso_pack, size;
1048                         if (e == 3) {
1049                                 // Allocate for minimum width (because that will give us the most
1050                                 // number of packets, so we don't need to reallocated, but we'll
1051                                 // default to 720p for the first frame.
1052                                 size = find_xfer_size_for_width(MIN_WIDTH);
1053                                 num_iso_pack = USB_VIDEO_TRANSFER_SIZE / size;
1054                                 buf_size = USB_VIDEO_TRANSFER_SIZE;
1055                         } else {
1056                                 size = 0xc0;
1057                                 num_iso_pack = 80;
1058                                 buf_size = num_iso_pack * size;
1059                         }
1060                         assert(size_t(num_iso_pack * size) <= buf_size);
1061                         uint8_t *buf = new uint8_t[buf_size];
1062
1063                         xfr = libusb_alloc_transfer(num_iso_pack);
1064                         if (!xfr) {
1065                                 fprintf(stderr, "oom\n");
1066                                 exit(1);
1067                         }
1068
1069                         int ep = LIBUSB_ENDPOINT_IN | e;
1070                         libusb_fill_iso_transfer(xfr, devh, ep, buf, buf_size,
1071                                 num_iso_pack, cb_xfr, nullptr, 0);
1072                         libusb_set_iso_packet_lengths(xfr, size);
1073                         xfr->user_data = this;
1074
1075                         if (e == 3) {
1076                                 change_xfer_size_for_width(assumed_frame_width, xfr);
1077                         }
1078
1079                         iso_xfrs.push_back(xfr);
1080                 }
1081         }
1082 }
1083
1084 void BMUSBCapture::start_bm_capture()
1085 {
1086         int i = 0;
1087         for (libusb_transfer *xfr : iso_xfrs) {
1088                 int rc = libusb_submit_transfer(xfr);
1089                 ++i;
1090                 if (rc < 0) {
1091                         //printf("num_bytes=%d\n", num_bytes);
1092                         fprintf(stderr, "Error submitting iso to endpoint 0x%02x, number %d: %s\n",
1093                                 xfr->endpoint, i, libusb_error_name(rc));
1094                         exit(1);
1095                 }
1096         }
1097
1098
1099 #if 0
1100         libusb_release_interface(devh, 0);
1101 out:
1102         if (devh)
1103                 libusb_close(devh);
1104         libusb_exit(nullptr);
1105         return rc;
1106 #endif
1107 }
1108
1109 void BMUSBCapture::stop_dequeue_thread()
1110 {
1111         dequeue_thread_should_quit = true;
1112         queues_not_empty.notify_all();
1113         dequeue_thread.join();
1114 }
1115
1116 void BMUSBCapture::start_bm_thread()
1117 {
1118         should_quit = false;
1119         usb_thread = thread(&BMUSBCapture::usb_thread_func);
1120 }
1121
1122 void BMUSBCapture::stop_bm_thread()
1123 {
1124         should_quit = true;
1125         usb_thread.join();
1126 }
1127
1128 struct VideoFormatEntry {
1129         uint16_t normalized_video_format;
1130         unsigned width, height, second_field_start;
1131         unsigned extra_lines_top, extra_lines_bottom;
1132         unsigned frame_rate_nom, frame_rate_den;
1133         bool interlaced;
1134 };
1135
1136 bool decode_video_format(uint16_t video_format, VideoFormat *decoded_video_format)
1137 {
1138         decoded_video_format->id = video_format;
1139         decoded_video_format->interlaced = false;
1140
1141         // TODO: Add these for all formats as we find them.
1142         decoded_video_format->extra_lines_top = decoded_video_format->extra_lines_bottom = decoded_video_format->second_field_start = 0;
1143
1144         if (video_format == 0x0800) {
1145                 // No video signal. These green pseudo-frames seem to come at about 30.13 Hz.
1146                 // It's a strange thing, but what can you do.
1147                 decoded_video_format->width = 720;
1148                 decoded_video_format->height = 525;
1149                 decoded_video_format->extra_lines_top = 0;
1150                 decoded_video_format->extra_lines_bottom = 0;
1151                 decoded_video_format->frame_rate_nom = 3013;
1152                 decoded_video_format->frame_rate_den = 100;
1153                 decoded_video_format->has_signal = false;
1154                 return true;
1155         }
1156         if ((video_format & 0xe800) != 0xe800) {
1157                 printf("Video format 0x%04x does not appear to be a video format. Assuming 60 Hz.\n",
1158                         video_format);
1159                 decoded_video_format->width = 0;
1160                 decoded_video_format->height = 0;
1161                 decoded_video_format->extra_lines_top = 0;
1162                 decoded_video_format->extra_lines_bottom = 0;
1163                 decoded_video_format->frame_rate_nom = 60;
1164                 decoded_video_format->frame_rate_den = 1;
1165                 decoded_video_format->has_signal = false;
1166                 return false;
1167         }
1168
1169         decoded_video_format->has_signal = true;
1170
1171         // NTSC (480i59.94, I suppose). A special case, see below.
1172         if (video_format == 0xe901 || video_format == 0xe9c1 || video_format == 0xe801) {
1173                 decoded_video_format->width = 720;
1174                 decoded_video_format->height = 480;
1175                 decoded_video_format->extra_lines_top = 17;
1176                 decoded_video_format->extra_lines_bottom = 28;
1177                 decoded_video_format->frame_rate_nom = 30000;
1178                 decoded_video_format->frame_rate_den = 1001;
1179                 decoded_video_format->second_field_start = 280;
1180                 decoded_video_format->interlaced = true;
1181                 return true;
1182         }
1183
1184         // PAL (576i50, I suppose). A special case, see below.
1185         if (video_format == 0xe909 || video_format == 0xe9c9 || video_format == 0xe809 || video_format == 0xebe9 || video_format == 0xebe1) {
1186                 decoded_video_format->width = 720;
1187                 decoded_video_format->height = 576;
1188                 decoded_video_format->extra_lines_top = 22;
1189                 decoded_video_format->extra_lines_bottom = 27;
1190                 decoded_video_format->frame_rate_nom = 25;
1191                 decoded_video_format->frame_rate_den = 1;
1192                 decoded_video_format->second_field_start = 335;
1193                 decoded_video_format->interlaced = true;
1194                 return true;
1195         }
1196
1197         // 0x8 seems to be a flag about availability of deep color on the input,
1198         // except when it's not (e.g. it's the only difference between NTSC
1199         // and PAL). Rather confusing. But we clear it here nevertheless, because
1200         // usually it doesn't mean anything.
1201         //
1202         // 0x4 is a flag I've only seen from the D4. I don't know what it is.
1203         uint16_t normalized_video_format = video_format & ~0xe80c;
1204         constexpr VideoFormatEntry entries[] = {
1205                 { 0x01f1,  720,  480,   0, 40,  5, 60000, 1001, false },  // 480p59.94 (believed).
1206                 { 0x0131,  720,  576,   0, 44,  5,    50,    1, false },  // 576p50.
1207                 { 0x0011,  720,  576,   0, 44,  5,    50,    1, false },  // 576p50 (5:4).
1208                 { 0x0143, 1280,  720,   0, 25,  5,    50,    1, false },  // 720p50.
1209                 { 0x0103, 1280,  720,   0, 25,  5,    60,    1, false },  // 720p60.
1210                 { 0x0125, 1280,  720,   0, 25,  5,    60,    1, false },  // 720p60.
1211                 { 0x0121, 1280,  720,   0, 25,  5, 60000, 1001, false },  // 720p59.94.
1212                 { 0x01c3, 1920, 1080,   0,  0,  0,    30,    1, false },  // 1080p30.
1213                 { 0x0003, 1920, 1080, 583, 20, 25,    30,    1,  true },  // 1080i60.
1214                 { 0x01e1, 1920, 1080,   0,  0,  0, 30000, 1001, false },  // 1080p29.97.
1215                 { 0x0021, 1920, 1080, 583, 20, 25, 30000, 1001,  true },  // 1080i59.94.
1216                 { 0x0063, 1920, 1080,   0,  0,  0,    25,    1, false },  // 1080p25.
1217                 { 0x0043, 1920, 1080,   0,  0,  0,    25,    1,  true },  // 1080p50.
1218                 { 0x008e, 1920, 1080,   0,  0,  0,    24,    1, false },  // 1080p24.
1219                 { 0x00a1, 1920, 1080,   0,  0,  0, 24000, 1001, false },  // 1080p23.98.
1220         };
1221         for (const VideoFormatEntry &entry : entries) {
1222                 if (normalized_video_format == entry.normalized_video_format) {
1223                         decoded_video_format->width = entry.width;
1224                         decoded_video_format->height = entry.height;
1225                         decoded_video_format->second_field_start = entry.second_field_start;
1226                         decoded_video_format->extra_lines_top = entry.extra_lines_top;
1227                         decoded_video_format->extra_lines_bottom = entry.extra_lines_bottom;
1228                         decoded_video_format->frame_rate_nom = entry.frame_rate_nom;
1229                         decoded_video_format->frame_rate_den = entry.frame_rate_den;
1230                         decoded_video_format->interlaced = entry.interlaced;
1231                         return true;
1232                 }
1233         }
1234
1235         printf("Unknown video format 0x%04x (normalized 0x%04x). Assuming 720p60.\n", video_format, normalized_video_format);
1236         decoded_video_format->width = 1280;
1237         decoded_video_format->height = 720;
1238         decoded_video_format->frame_rate_nom = 60;
1239         decoded_video_format->frame_rate_den = 1;
1240         return false;
1241 }