From c478ee1edc190ac322aba229400e11541823223e Mon Sep 17 00:00:00 2001 From: Steve Markgraf Date: Thu, 27 Feb 2025 23:05:48 +0100 Subject: [PATCH] Support for format conversion and multiple streams --- include/format_convert.h | 9 ++ include/hsdaoh.h | 11 ++- include/hsdaoh_private.h | 94 +++++++++++++++++++ src/CMakeLists.txt | 4 +- src/format_convert.c | 191 +++++++++++++++++++++++++++++++++++++++ src/hsdaoh_file.c | 132 ++++++++++++++++++--------- src/hsdaoh_tcp.c | 3 + src/hsdaoh_test.c | 3 + src/libhsdaoh.c | 112 +++++++++++------------ 9 files changed, 453 insertions(+), 106 deletions(-) create mode 100644 include/format_convert.h create mode 100644 include/hsdaoh_private.h create mode 100644 src/format_convert.c diff --git a/include/format_convert.h b/include/format_convert.h new file mode 100644 index 0000000..92aa053 --- /dev/null +++ b/include/format_convert.h @@ -0,0 +1,9 @@ +#ifndef __FORMAT_CONVERT_H +#define __FORMAT_CONVERT_H + +void hsdaoh_unpack_pio_12bit(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info); +void hsdaoh_unpack_pio_12bit_dual(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info); +void hsdaoh_unpack_fpga_12bit_dual(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info); +void hsdaoh_unpack_pio_pcm1802_audio(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info); + +#endif diff --git a/include/hsdaoh.h b/include/hsdaoh.h index f1c374c..350a1af 100644 --- a/include/hsdaoh.h +++ b/include/hsdaoh.h @@ -34,16 +34,25 @@ extern "C" { #include #include +#define HSDAOH_MAX_BUF_SIZE (1920 * 1080 * 2) + typedef struct hsdaoh_data_info { void *ctx; unsigned char *buf; - uint32_t len; /* buffer length */ + size_t len; /* buffer length */ uint16_t stream_id; bool device_error; /* device error happened, terminate application */ } hsdaoh_data_info_t; typedef struct hsdaoh_dev hsdaoh_dev_t; +typedef enum +{ + OUT_FMT_RAW, + OUT_FMT_UNPACKED, + OUT_FMT_FLOAT +} hsdaoh_output_format_t; + HSDAOH_API uint32_t hsdaoh_get_device_count(void); HSDAOH_API const char* hsdaoh_get_device_name(uint32_t index); diff --git a/include/hsdaoh_private.h b/include/hsdaoh_private.h new file mode 100644 index 0000000..3629dc8 --- /dev/null +++ b/include/hsdaoh_private.h @@ -0,0 +1,94 @@ +#ifndef __HSDAOH_PRIVATE_H +#define __HSDAOH_PRIVATE_H + +enum hsdaoh_async_status { + HSDAOH_INACTIVE = 0, + HSDAOH_CANCELING, + HSDAOH_RUNNING +}; + +struct hsdaoh_dev { + libusb_context *ctx; + struct libusb_device_handle *devh; + hsdaoh_read_cb_t cb; + void *cb_ctx; + enum hsdaoh_async_status async_status; + int async_cancel; + uint16_t vid; + uint16_t pid; + + /* UVC related */ + uvc_context_t *uvc_ctx; + uvc_device_t *uvc_dev; + uvc_device_handle_t *uvc_devh; + + int hid_interface; + + uint8_t edid_seq_cnt; + int frames_since_error; + int discard_start_frames; + unsigned int in_order_cnt; + uint16_t last_frame_cnt; + uint16_t last_crc[2]; + uint16_t idle_cnt; + bool stream_synced; + + unsigned int width, height, fps; + + bool output_float; + + /* status */ + int dev_lost; + bool driver_active; + unsigned int xfer_errors; + char manufact[256]; + char product[256]; +}; + +enum +{ + RAW_8BIT, + RAW_16BIT, + RAW_24BIT, + RAW_32BIT, + RAW_64BIT, + PIO_1BIT, + PIO_2BIT, + PIO_3BIT, + PIO_4BIT, + PIO_5BIT, + PIO_6BIT, + PIO_7BIT, + PIO_8BIT, + PIO_8BIT_DUAL, + PIO_8BIT_IQ, + PIO_9BIT, + PIO_10BIT, + PIO_10BIT_DUAL, + PIO_10BIT_IQ, + PIO_11BIT, + PIO_12BIT, + PIO_12BIT_DUAL, + PIO_12BIT_IQ, + PIO_13BIT, + PIO_14BIT, + PIO_14BIT_DUAL, + PIO_14BIT_IQ, + PIO_15BIT, + PIO_16BIT, + PIO_16BIT_DUAL, + PIO_16BIT_IQ, + PIO_17BIT, + PIO_18BIT, + PIO_19BIT, + PIO_20BIT, + PIO_24BIT, + PIO_24BIT_IQ, + PIO_28BIT, + PIO_32BIT, + PIO_32BIT_IQ, + PIO_PCM1802_AUDIO, + // Placeholder for internal ADC data from pico +}; + +#endif diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a493583..95637fa 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -18,7 +18,7 @@ ######################################################################## # Setup shared library variant ######################################################################## -add_library(hsdaoh SHARED libhsdaoh.c) +add_library(hsdaoh SHARED libhsdaoh.c format_convert.c) target_link_libraries(hsdaoh ${LIBUSB_LIBRARIES} ${LIBUVC_LIBRARIES} ${THREADS_PTHREADS_LIBRARY}) target_include_directories(hsdaoh PUBLIC $ @@ -36,7 +36,7 @@ generate_export_header(hsdaoh) ######################################################################## # Setup static library variant ######################################################################## -add_library(hsdaoh_static STATIC libhsdaoh.c) +add_library(hsdaoh_static STATIC libhsdaoh.c format_convert.c) target_link_libraries(hsdaoh m ${LIBUSB_LIBRARIES} ${LIBUVC_LIBRARIES} ${THREADS_PTHREADS_LIBRARY}) target_include_directories(hsdaoh_static PUBLIC $ diff --git a/src/format_convert.c b/src/format_convert.c new file mode 100644 index 0000000..af7536b --- /dev/null +++ b/src/format_convert.c @@ -0,0 +1,191 @@ +/* + * hsdaoh - High Speed Data Acquisition over MS213x USB3 HDMI capture sticks + * Format conversion routines + * + * Copyright (C) 2024-2025 by Steve Markgraf + * + * SPDX-License-Identifier: GPL-2.0+ + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +static inline void hsdaoh_16bit_to_float(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info, uint16_t *buf, size_t length, float scale) +{ + unsigned int i, j = 0; + + float *floats = malloc(sizeof(float) * dev->width * dev->height * 2 * 2); + + if (!floats) + return; + + for (unsigned int i = 0; i < length; i++) { + float sample_i = buf[i]; + floats[j++] = (sample_i - scale) * (1/scale); + floats[j++] = (sample_i - scale) * (1/scale); + } + + data_info->buf = (uint8_t *)floats; + data_info->len = j * sizeof(float); + dev->cb(data_info); + + free(floats); +} + +// We receive three 16-bit words containing four 12-bit samples (sample A - D) +// First word: A03 A02 A01 A00 B11 B10 B09 B08 B07 B06 B05 B04 B03 B02 B01 B00 +// Second word: A07 A06 A05 A04 C11 C10 C09 C08 C07 C06 C05 C04 C03 C02 C01 C00 +// Third word: A11 A10 A09 A08 D11 D10 D09 D08 D07 D06 D05 D04 D03 D02 D01 D00 +void hsdaoh_unpack_pio_12bit(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info) +{ + uint16_t *in = (uint16_t *)data_info->buf; + size_t inlen = data_info->len / sizeof(uint16_t); + uint16_t *out = malloc(sizeof(uint16_t) * dev->width * dev->height * 2); + unsigned int j = 0; + + for (unsigned int i = 0; i < inlen; i += 3) { + out[j++] = (in[i+2] & 0xf000) >> 4 | (in[i+1] & 0xf000) >> 8 | (in[i] >> 12); + out[j++] = in[i ] & 0x0fff; + out[j++] = in[i+1] & 0x0fff; + out[j++] = in[i+2] & 0x0fff; + } + + if (dev->output_float) { + hsdaoh_16bit_to_float(dev, data_info, out, j, 2047.5); + } else { + data_info->buf = (uint8_t *)out; + data_info->len = j * sizeof(uint16_t); + dev->cb(data_info); + } + + free(out); +} + +// We receive three 32-bit words containing four 24-bit samples (sample A - D) +// First word: A07 A06 A05 A04 A03 A02 A01 A00 B23 B22 B21 B20 B19 B18 B17 B16 B15 B14 B13 B12 B11 B10 B09 B08 B07 B06 B05 B04 B03 B02 B01 B00 +// Second word: A15 A14 A13 A12 A11 A10 A09 A08 C23 C22 C21 C20 C19 C18 C17 C16 C15 C14 C13 C12 C11 C10 C09 C08 C07 C06 C05 C04 C03 C02 C01 C00 +// Third word: A23 A22 A21 A20 A19 A18 A17 A16 D23 D22 D21 D20 D19 D18 D17 D16 D15 D14 D13 D12 D11 D10 D09 D08 D07 D06 D05 D04 D03 D02 D01 D00 +void hsdaoh_unpack_pio_12bit_dual(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info) +{ + uint32_t *in = (uint32_t *)data_info->buf; + size_t inlen = data_info->len / sizeof(uint32_t); + uint32_t *out = malloc(sizeof(uint32_t) * dev->width * dev->height * 2); + uint16_t *out16_1 = malloc(sizeof(uint16_t) * dev->width * dev->height * 2); + uint16_t *out16_2 = malloc(sizeof(uint16_t) * dev->width * dev->height * 2); + unsigned int i = 0, j = 0; + + for (i = 0; i < inlen; i += 3) { + out[j++] = (in[i+2] & 0xff000000) >> 8 | (in[i+1] & 0xff000000) >> 16 | (in[i] >> 24); + out[j++] = in[i ] & 0x00ffffff; + out[j++] = in[i+1] & 0x00ffffff; + out[j++] = in[i+2] & 0x00ffffff; + } + + for (i = 0; i < j; i++) { + out16_1[i] = (out[i] >> 12) & 0x0fff; + out16_2[i] = out[i] & 0x0fff; + } + + if (dev->output_float) { + hsdaoh_16bit_to_float(dev, data_info, out16_1, i, 2047.5); + } else { + data_info->buf = (uint8_t *)out16_1; + data_info->len = i * sizeof(uint16_t); + + dev->cb(data_info); + } + + data_info->stream_id += 1; + + if (dev->output_float) { + hsdaoh_16bit_to_float(dev, data_info, out16_2, i, 2047.5); + } else { + data_info->buf = (uint8_t *)out16_2; + data_info->len = i * sizeof(uint16_t); + + dev->cb(data_info); + } + + free(out); + free(out16_1); + free(out16_2); +} + +void hsdaoh_unpack_fpga_12bit_dual(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info) +{ + uint16_t *in = (uint16_t *)data_info->buf; + size_t inlen = data_info->len / sizeof(uint16_t); + unsigned int i, j = 0; + uint16_t *out16_1 = malloc(sizeof(uint16_t) * inlen * 2); + uint16_t *out16_2 = malloc(sizeof(uint16_t) * inlen * 2); + + if (!out16_1 || !out16_2) + return; + + /* extract packed 2x12 bit samples */ + for (i = 0; i < inlen; i += 3) { + uint16_t lsbs = in[i+2]; + out16_1[j] = ((in[i+0] & 0xff00) >> 4) | ((lsbs >> 0) & 0xf); + out16_2[j++] = ((in[i+0] & 0x00ff) << 4) | ((lsbs >> 4) & 0xf); + out16_1[j] = ((in[i+1] & 0xff00) >> 4) | ((lsbs >> 8) & 0xf); + out16_2[j++] = ((in[i+1] & 0x00ff) << 4) | ((lsbs >> 12) & 0xf); + } + + if (dev->output_float) { + hsdaoh_16bit_to_float(dev, data_info, out16_1, i, 2047.5); + } else { + data_info->buf = (uint8_t *)out16_1; + data_info->len = i * sizeof(uint16_t); + + dev->cb(data_info); + } + + data_info->stream_id += 1; + + if (dev->output_float) { + hsdaoh_16bit_to_float(dev, data_info, out16_2, i, 2047.5); + } else { + data_info->buf = (uint8_t *)out16_2; + data_info->len = i * sizeof(uint16_t); + + dev->cb(data_info); + } + + free(out16_1); + free(out16_2); +} + +void hsdaoh_unpack_pio_pcm1802_audio(hsdaoh_dev_t *dev, hsdaoh_data_info_t *data_info) +{ + uint32_t *in = (uint32_t *)data_info->buf; + size_t inlen = data_info->len / sizeof(uint32_t); + + /* convert from S24LE to S32LE */ + for (unsigned int i = 0; i < inlen; i++) + in[i] <<= 8; + + data_info->buf = (uint8_t *)in; + data_info->len = inlen * sizeof(uint32_t); + + dev->cb(data_info); +} diff --git a/src/hsdaoh_file.c b/src/hsdaoh_file.c index 454cac0..33133e1 100644 --- a/src/hsdaoh_file.c +++ b/src/hsdaoh_file.c @@ -37,10 +37,16 @@ #include "hsdaoh.h" +#define FD_NUMS 4 + static int do_exit = 0; static uint32_t bytes_to_read = 0; static hsdaoh_dev_t *dev = NULL; +typedef struct file_ctx { + FILE *files[FD_NUMS]; +} file_ctx_t; + void usage(void) { fprintf(stderr, @@ -49,7 +55,8 @@ void usage(void) "\t[-d device_index (default: 0)]\n" "\t[-p ppm_error (default: 0)]\n" "\t[-n number of samples to read (default: 0, infinite)]\n" - "\tfilename (a '-' dumps samples to stdout)\n\n"); + "\t[-0 to -3 filename of steam 0 to stream 3 (a '-' dumps samples to stdout)]\n" + "\tfilename (of stream 0) (a '-' dumps samples to stdout)\n\n"); exit(1); } @@ -75,37 +82,41 @@ static void sighandler(int signum) } #endif -void hsdaoh_callback(hsdaoh_data_info_t *data_info) +static void hsdaoh_callback(hsdaoh_data_info_t *data_info) { - unsigned char *buf = data_info->buf; - uint32_t len = data_info->len; - void *ctx = data_info->ctx; size_t nbytes = 0; + uint32_t len = data_info->len; - if (ctx) { - if (do_exit) - return; + if (!data_info->ctx || do_exit) + return; - if ((bytes_to_read > 0) && (bytes_to_read < len)) { - len = bytes_to_read; - do_exit = 1; - hsdaoh_stop_stream(dev); - } + if (data_info->stream_id >= FD_NUMS) + return; - while (nbytes < len) { - nbytes += fwrite(buf + nbytes, 1, len - nbytes, (FILE*)ctx); + file_ctx_t *f = (file_ctx_t *)data_info->ctx; + FILE *file = f->files[data_info->stream_id]; - if (ferror((FILE*)ctx)) { - fprintf(stderr, "Error writing file, samples lost, exiting!\n"); - hsdaoh_stop_stream(dev); - break; - } + if (!file) + return; - } - - if (bytes_to_read > 0) - bytes_to_read -= len; + if ((bytes_to_read > 0) && (bytes_to_read < len)) { + len = bytes_to_read; + do_exit = 1; + hsdaoh_stop_stream(dev); } + + while (nbytes < len) { + nbytes += fwrite(data_info->buf + nbytes, 1, len - nbytes, file); + + if (ferror(file)) { + fprintf(stderr, "Error writing file, samples lost, exiting!\n"); + hsdaoh_stop_stream(dev); + break; + } + } + + if (bytes_to_read > 0) + bytes_to_read -= len; } int main(int argc, char **argv) @@ -113,14 +124,16 @@ int main(int argc, char **argv) #ifndef _WIN32 struct sigaction sigact; #endif - char *filename = NULL; + char *filenames[FD_NUMS] = { NULL, }; int n_read; int r, opt; int ppm_error = 0; - FILE *file; + file_ctx_t f; int dev_index = 0; + bool fname0_used = false; + bool have_file = false; - while ((opt = getopt(argc, argv, "d:n:p:d:")) != -1) { + while ((opt = getopt(argc, argv, "0:1:2:3:d:n:p:d:a:")) != -1) { switch (opt) { case 'd': dev_index = (uint32_t)atoi(optarg); @@ -131,21 +144,40 @@ int main(int argc, char **argv) case 'n': bytes_to_read = (uint32_t)atof(optarg) * 2; break; + case '0': + fname0_used = true; + have_file = true; + filenames[0] = optarg; + break; + case '1': + have_file = true; + filenames[1] = optarg; + break; + case '2': + have_file = true; + filenames[2] = optarg; + break; + case '3': + have_file = true; + filenames[3] = optarg; + break; default: usage(); break; } } - if (argc <= optind) { - usage(); - } else { - filename = argv[optind]; + if (!fname0_used) { + if (argc <= optind) { + if (!have_file) + usage(); + } else { + filenames[0] = argv[optind]; + } } - if (dev_index < 0) { + if (dev_index < 0) exit(1); - } r = hsdaoh_open(&dev, (uint32_t)dev_index); if (r < 0) { @@ -164,21 +196,28 @@ int main(int argc, char **argv) SetConsoleCtrlHandler( (PHANDLER_ROUTINE) sighandler, TRUE ); #endif - if (strcmp(filename, "-") == 0) { /* Write samples to stdout */ - file = stdout; + for (int i = 0; i < FD_NUMS; i++) { + f.files[i] = NULL; + + if (!filenames[i]) + continue; + + if (strcmp(filenames[i], "-") == 0) { /* Write samples to stdout */ + f.files[i] = stdout; #ifdef _WIN32 - _setmode(_fileno(stdin), _O_BINARY); + _setmode(_fileno(stdin), _O_BINARY); #endif - } else { - file = fopen(filename, "wb"); - if (!file) { - fprintf(stderr, "Failed to open %s\n", filename); - goto out; + } else { + f.files[i] = fopen(filenames[i], "wb"); + if (!f.files[i]) { + fprintf(stderr, "Failed to open %s\n", filenames[i]); + goto out; + } } } fprintf(stderr, "Reading samples...\n"); - r = hsdaoh_start_stream(dev, hsdaoh_callback, (void *)file); + r = hsdaoh_start_stream(dev, hsdaoh_callback, (void *)&f); while (!do_exit) { usleep(50000); @@ -189,10 +228,13 @@ int main(int argc, char **argv) else fprintf(stderr, "\nLibrary error %d, exiting...\n", r); - if (file != stdout) - fclose(file); - hsdaoh_close(dev); + + for (int i = 0; i < FD_NUMS; i++) { + if (f.files[i] && (f.files[i] != stdout)) + fclose(f.files[i]); + } + out: return r >= 0 ? r : -r; } diff --git a/src/hsdaoh_tcp.c b/src/hsdaoh_tcp.c index c0b6d66..fce01f8 100644 --- a/src/hsdaoh_tcp.c +++ b/src/hsdaoh_tcp.c @@ -153,6 +153,9 @@ void hsdaoh_callback(hsdaoh_data_info_t *data_info) uint32_t len = data_info->len; void *ctx = data_info->ctx; + if (data_info->stream_id != 0) + return; + if(!do_exit) { struct llist *rpt = (struct llist*)malloc(sizeof(struct llist)); rpt->data = (char*)malloc(len); diff --git a/src/hsdaoh_test.c b/src/hsdaoh_test.c index 9d8a0a7..8c72b41 100644 --- a/src/hsdaoh_test.c +++ b/src/hsdaoh_test.c @@ -228,6 +228,9 @@ static void hsdaoh_callback(hsdaoh_data_info_t *data_info) uint32_t len = data_info->len; void *ctx = data_info->ctx; + if (data_info->stream_id != 0) + return; + /* verify the counter value */ uint16_t *cnt = (uint16_t *)buf; int n = len / sizeof(uint16_t); diff --git a/src/libhsdaoh.c b/src/libhsdaoh.c index dafbd11..4ac651f 100644 --- a/src/libhsdaoh.c +++ b/src/libhsdaoh.c @@ -42,51 +42,12 @@ #include #include #include + #include +#include +#include #include -enum hsdaoh_async_status { - HSDAOH_INACTIVE = 0, - HSDAOH_CANCELING, - HSDAOH_RUNNING -}; - -struct hsdaoh_dev { - libusb_context *ctx; - struct libusb_device_handle *devh; - hsdaoh_read_cb_t cb; - void *cb_ctx; - enum hsdaoh_async_status async_status; - int async_cancel; - uint16_t vid; - uint16_t pid; - - /* UVC related */ - uvc_context_t *uvc_ctx; - uvc_device_t *uvc_dev; - uvc_device_handle_t *uvc_devh; - - int hid_interface; - - uint8_t edid_seq_cnt; - int frames_since_error; - int discard_start_frames; - unsigned int in_order_cnt; - uint16_t last_frame_cnt; - uint16_t last_crc[2]; - uint16_t idle_cnt; - bool stream_synced; - - unsigned int width, height, fps; - - /* status */ - int dev_lost; - bool driver_active; - unsigned int xfer_errors; - char manufact[256]; - char product[256]; -}; - typedef struct hsdaoh_adapter { uint16_t vid; uint16_t pid; @@ -95,7 +56,7 @@ typedef struct hsdaoh_adapter { static hsdaoh_adapter_t known_devices[] = { { 0x345f, 0x2130, "MS2130" }, - { 0x534d, 0x2130, "MS2130 OEM?" }, + { 0x534d, 0x2130, "MS2130 OEM" }, { 0x345f, 0x2131, "MS2131" }, }; @@ -109,12 +70,15 @@ typedef struct { uint32_t magic; uint16_t framecounter; - uint8_t pack_state; + uint8_t reserved1; uint8_t crc_config; - uint8_t data_width; - uint8_t data_signedness; + uint16_t version; + uint32_t flags; } __attribute__((packed, aligned(1))) metadata_t; +#define FLAG_STREAM_ID_PRESENT (1 << 0) +#define FLAG_FORMAT_ID_PRESENT (1 << 1) + #define CTRL_TIMEOUT 300 int hsdaoh_get_hid_feature_report(hsdaoh_dev_t *dev, unsigned char *data, size_t length) @@ -573,6 +537,30 @@ inline void hsdaoh_extract_metadata(uint8_t *data, metadata_t *metadata, unsigne meta[j++] = (data[((i+1)*width*2) - 1] >> 4) | (data[((i+2)*width*2) - 1] & 0xf0); } +void hsdaoh_output(hsdaoh_dev_t *dev, uint16_t sid, int format, uint8_t *data, size_t len) +{ + hsdaoh_data_info_t data_info; + data_info.ctx = dev->cb_ctx; + data_info.stream_id = sid; + data_info.buf = data; + data_info.len = len; + + switch (format) { + case PIO_12BIT: + hsdaoh_unpack_pio_12bit(dev, &data_info); + break; + case PIO_12BIT_DUAL: + hsdaoh_unpack_pio_12bit_dual(dev, &data_info); + break; + case PIO_PCM1802_AUDIO: + hsdaoh_unpack_pio_pcm1802_audio(dev, &data_info); + break; + default: + dev->cb(&data_info); + break; + } +} + void hsdaoh_process_frame(hsdaoh_dev_t *dev, uint8_t *data, int size) { uint32_t frame_payload_bytes = 0; @@ -602,6 +590,7 @@ void hsdaoh_process_frame(hsdaoh_dev_t *dev, uint8_t *data, int size) dev->last_frame_cnt = meta.framecounter; int frame_errors = 0; + uint16_t stream0_format = 0; for (unsigned int i = 0; i < dev->height; i++) { uint8_t *line_dat = data + (dev->width * sizeof(uint16_t) * i); @@ -610,6 +599,12 @@ void hsdaoh_process_frame(hsdaoh_dev_t *dev, uint8_t *data, int size) uint16_t payload_len = le16toh(((uint16_t *)line_dat)[dev->width - 1]); uint16_t crc = le16toh(((uint16_t *)line_dat)[dev->width - 2]); uint16_t stream_id = le16toh(((uint16_t *)line_dat)[dev->width - 3]); + uint16_t format = (meta.flags & FLAG_FORMAT_ID_PRESENT) ? stream_id >> 6 : RAW_8BIT; + + if (meta.flags & FLAG_STREAM_ID_PRESENT) + stream_id &= 0x3f; + else + stream_id = 0; /* we only use 12 bits, the upper 4 bits are reserved for the metadata */ payload_len &= 0x0fff; @@ -635,20 +630,19 @@ void hsdaoh_process_frame(hsdaoh_dev_t *dev, uint8_t *data, int size) dev->last_crc[0] = crc16_ccitt(line_dat, dev->width * sizeof(uint16_t)); } - if (payload_len > 0) - memmove(data + frame_payload_bytes, line_dat, payload_len * sizeof(uint16_t)); - - frame_payload_bytes += payload_len * sizeof(uint16_t); + if ((payload_len > 0) && dev->stream_synced) { + if (!(meta.flags & FLAG_STREAM_ID_PRESENT) || stream_id == 0) { + memmove(data + frame_payload_bytes, line_dat, payload_len * sizeof(uint16_t)); + frame_payload_bytes += payload_len * sizeof(uint16_t); + stream0_format = format; + } else { + hsdaoh_output(dev, stream_id, format, line_dat, payload_len * sizeof(uint16_t)); + } + } } - hsdaoh_data_info_t data_info; - data_info.stream_id = 0; - data_info.buf = (uint8_t *)data; - data_info.len = frame_payload_bytes; - data_info.ctx = dev->cb_ctx; - - if (dev->cb && dev->stream_synced) - dev->cb(&data_info); + if (dev->stream_synced && frame_payload_bytes) + hsdaoh_output(dev, 0, stream0_format, data, frame_payload_bytes); if (frame_errors && dev->stream_synced) { fprintf(stderr,"%d frame errors, %d frames since last error\n", frame_errors, dev->frames_since_error); @@ -703,6 +697,8 @@ int hsdaoh_start_stream(hsdaoh_dev_t *dev, hsdaoh_read_cb_t cb, void *ctx) dev->cb = cb; dev->cb_ctx = ctx; +// dev->output_float = true; + uvc_error_t res; uvc_stream_ctrl_t ctrl;