diff options
Diffstat (limited to 'video-ps3eye/PS3EYEDriver')
24 files changed, 2157 insertions, 67 deletions
diff --git a/video-ps3eye/PS3EYEDriver/.gitignore b/video-ps3eye/PS3EYEDriver/.gitignore new file mode 100644 index 00000000..4066da7e --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/.gitignore @@ -0,0 +1,2 @@ +/build*/ +/msvc-binary/ diff --git a/video-ps3eye/PS3EYEDriver/CMakeLists.txt b/video-ps3eye/PS3EYEDriver/CMakeLists.txt new file mode 100644 index 00000000..4e30d2b4 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/CMakeLists.txt @@ -0,0 +1,3 @@ +link_libraries(libusb-1.0) +otr_module(ps3eye STATIC NO-QT) +add_subdirectory("sdl") diff --git a/video-ps3eye/PS3EYEDriver/LICENSE b/video-ps3eye/PS3EYEDriver/LICENSE new file mode 100644 index 00000000..ac8ed3a1 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/LICENSE @@ -0,0 +1,47 @@ + +License information for PS3EYEDriver +------------------------------------ + +The license of the PS3EYEDriver is MIT (for newly-written code) and GPLv2 for +all code derived from the Linux Kernel Driver (ov534) sources. + +In https://github.com/inspirit/PS3EYEDriver/pull/3, Eugene Zatepyakin writes: + + "all of my code is MIT licensed and to tell the truth i didnt check Linux + p3eye version license. as far as i know it was contributed to Linux by some + devs who decided to do it on their own..." + +The code is based on the Linux driver for the PSEye, which can be found here: + +http://git.kernel.org/cgit/linux/kernel/git/torvalds/linux.git/tree/drivers/media/usb/gspca/ov534.c + +/* + * ov534-ov7xxx gspca driver + * + * Copyright (C) 2008 Antonio Ospite <ospite@studenti.unina.it> + * Copyright (C) 2008 Jim Paris <jim@jtan.com> + * Copyright (C) 2009 Jean-Francois Moine http://moinejf.free.fr + * + * Based on a prototype written by Mark Ferrell <majortrips@gmail.com> + * USB protocol reverse engineered by Jim Paris <jim@jtan.com> + * https://jim.sh/svn/jim/devl/playstation/ps3/eye/test/ + * + * PS3 Eye camera enhanced by Richard Kaswy http://kaswy.free.fr + * PS3 Eye camera - brightness, contrast, awb, agc, aec controls + * added by Max Thrun <bear24rw@gmail.com> + * + * 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 + * 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, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + diff --git a/video-ps3eye/PS3EYEDriver/LICENSE.opentrack b/video-ps3eye/PS3EYEDriver/LICENSE.opentrack new file mode 100644 index 00000000..65aa9106 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/LICENSE.opentrack @@ -0,0 +1,17 @@ +Licensing for PS3EYEDriver modifications: + +--- + +Copyright (c) 2019, Stanislaw Halik <sthalik@misaki.pl> + +Permission to use, copy, modify, and/or distribute this software for any +purpose with or without fee is hereby granted, provided that the above +copyright notice and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/video-ps3eye/PS3EYEDriver/constants.cpp b/video-ps3eye/PS3EYEDriver/constants.cpp new file mode 100644 index 00000000..17fbd4b2 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/constants.cpp @@ -0,0 +1,198 @@ +#include "constants.hpp" + +#define OV534_REG_ADDRESS 0xf1 /* sensor address */ + +// init + +const uint8_t ov534_reg_initdata[47][2] = { + { 0xe7, 0x3a }, + + { OV534_REG_ADDRESS, 0x42 }, /* select OV772x sensor */ + + { 0x92, 0x01 }, + { 0x93, 0x18 }, + { 0x94, 0x10 }, + { 0x95, 0x10 }, + { 0xE2, 0x00 }, + { 0xE7, 0x3E }, + + { 0x96, 0x00 }, + { 0x97, 0x20 }, + { 0x97, 0x20 }, + { 0x97, 0x20 }, + { 0x97, 0x0A }, + { 0x97, 0x3F }, + { 0x97, 0x4A }, + { 0x97, 0x20 }, + { 0x97, 0x15 }, + { 0x97, 0x0B }, + + { 0x8E, 0x40 }, + { 0x1F, 0x81 }, + { 0xC0, 0x50 }, + { 0xC1, 0x3C }, + { 0xC2, 0x01 }, + { 0xC3, 0x01 }, + { 0x50, 0x89 }, + { 0x88, 0x08 }, + { 0x8D, 0x00 }, + { 0x8E, 0x00 }, + + { 0x1C, 0x00 }, /* video data start (V_FMT) */ + + { 0x1D, 0x00 }, /* RAW8 mode */ + { 0x1D, 0x02 }, /* payload size 0x0200 * 4 = 2048 bytes */ + { 0x1D, 0x00 }, /* payload size */ + + { 0x1D, 0x01 }, /* frame size = 0x012C00 * 4 = 307200 bytes (640 * 480 @ 8bpp) */ + { 0x1D, 0x2C }, /* frame size */ + { 0x1D, 0x00 }, /* frame size */ + + { 0x1C, 0x0A }, /* video data start (V_CNTL0) */ + { 0x1D, 0x08 }, /* turn on UVC header */ + { 0x1D, 0x0E }, + + { 0x34, 0x05 }, + { 0xE3, 0x04 }, + { 0x89, 0x00 }, + { 0x76, 0x00 }, + { 0xE7, 0x2E }, + { 0x31, 0xF9 }, + { 0x25, 0x42 }, + { 0x21, 0xF0 }, + { 0xE5, 0x04 } +}; + +const uint8_t ov772x_reg_initdata[35][2] = { + + { 0x12, 0x80 }, /* reset */ + { 0x3D, 0x00 }, + + { 0x12, 0x01 }, /* Processed Bayer RAW (8bit) */ + + { 0x11, 0x01 }, + { 0x14, 0x40 }, + { 0x15, 0x00 }, + { 0x63, 0xAA }, // AWB + { 0x64, 0x87 }, + { 0x66, 0x00 }, + { 0x67, 0x02 }, + { 0x17, 0x26 }, + { 0x18, 0xA0 }, + { 0x19, 0x07 }, + { 0x1A, 0xF0 }, + { 0x29, 0xA0 }, + { 0x2A, 0x00 }, + { 0x2C, 0xF0 }, + { 0x20, 0x10 }, + { 0x4E, 0x0F }, + { 0x3E, 0xF3 }, + { 0x0D, 0x41 }, + { 0x32, 0x00 }, + { 0x13, 0xF0 }, // COM8 - jfrancois 0xf0 orig x0f7 + { 0x22, 0x7F }, + { 0x23, 0x03 }, + { 0x24, 0x40 }, + { 0x25, 0x30 }, + { 0x26, 0xA1 }, + { 0x2A, 0x00 }, + { 0x2B, 0x00 }, + { 0x13, 0xF7 }, + { 0x0C, 0xC0 }, + + { 0x11, 0x00 }, + { 0x0D, 0x41 }, + + { 0x8E, 0x00 }, // De-noise threshold - jfrancois 0x00 - orig 0x04 +}; + +const uint8_t bridge_start_vga[9][2] = { + {0x1c, 0x00}, + {0x1d, 0x00}, + {0x1d, 0x02}, + {0x1d, 0x00}, + {0x1d, 0x01}, /* frame size = 0x012C00 * 4 = 307200 bytes (640 * 480 @ 8bpp) */ + {0x1d, 0x2C}, /* frame size */ + {0x1d, 0x00}, /* frame size */ + {0xc0, 0x50}, + {0xc1, 0x3c}, +}; +const uint8_t sensor_start_vga[8][2] = { + {0x12, 0x01}, + {0x17, 0x26}, + {0x18, 0xa0}, + {0x19, 0x07}, + {0x1a, 0xf0}, + {0x29, 0xa0}, + {0x2c, 0xf0}, + {0x65, 0x20}, +}; +const uint8_t bridge_start_qvga[9][2] = { + {0x1c, 0x00}, + {0x1d, 0x00}, + {0x1d, 0x02}, + {0x1d, 0x00}, + {0x1d, 0x00}, /* frame size = 0x004B00 * 4 = 76800 bytes (320 * 240 @ 8bpp) */ + {0x1d, 0x4b}, /* frame size */ + {0x1d, 0x00}, /* frame size */ + {0xc0, 0x28}, + {0xc1, 0x1e}, +}; +const uint8_t sensor_start_qvga[8][2] = { + {0x12, 0x41}, + {0x17, 0x3f}, + {0x18, 0x50}, + {0x19, 0x03}, + {0x1a, 0x78}, + {0x29, 0x50}, + {0x2c, 0x78}, + {0x65, 0x2f}, +}; + +// framerate selection + +const struct rate_s rate_0[14] = +{ + /* 640x480 */ + {83, 0x01, 0xc1, 0x02}, /* 83 FPS: video is partly corrupt */ + {75, 0x01, 0x81, 0x02}, /* 75 FPS or below: video is valid */ + {60, 0x00, 0x41, 0x04}, + {50, 0x01, 0x41, 0x02}, + {40, 0x02, 0xc1, 0x04}, + {30, 0x04, 0x81, 0x02}, + {25, 0x00, 0x01, 0x02}, + {20, 0x04, 0x41, 0x02}, + {15, 0x09, 0x81, 0x02}, + {10, 0x09, 0x41, 0x02}, + {8, 0x02, 0x01, 0x02}, + {5, 0x04, 0x01, 0x02}, + {3, 0x06, 0x01, 0x02}, + {2, 0x09, 0x01, 0x02}, +}; + +const struct rate_s rate_1[22] = +{ + /* 320x240 */ + {290, 0x00, 0xc1, 0x04}, + {205, 0x01, 0xc1, 0x02}, /* 205 FPS or above: video is partly corrupt */ + {187, 0x01, 0x81, 0x02}, /* 187 FPS or below: video is valid */ + {150, 0x00, 0x41, 0x04}, + {137, 0x02, 0xc1, 0x02}, + {125, 0x01, 0x41, 0x02}, + {100, 0x02, 0xc1, 0x04}, + {90, 0x03, 0x81, 0x02}, + {75, 0x04, 0x81, 0x02}, + {60, 0x04, 0xc1, 0x04}, + {50, 0x04, 0x41, 0x02}, + {40, 0x06, 0x81, 0x03}, + {37, 0x00, 0x01, 0x04}, + {30, 0x04, 0x41, 0x04}, + {17, 0x18, 0xc1, 0x02}, + {15, 0x18, 0x81, 0x02}, + {12, 0x02, 0x01, 0x04}, + {10, 0x18, 0x41, 0x02}, + {7, 0x04, 0x01, 0x04}, + {5, 0x06, 0x01, 0x04}, + {3, 0x09, 0x01, 0x04}, + {2, 0x18, 0x01, 0x02}, +}; diff --git a/video-ps3eye/PS3EYEDriver/constants.hpp b/video-ps3eye/PS3EYEDriver/constants.hpp new file mode 100644 index 00000000..5ae8d30c --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/constants.hpp @@ -0,0 +1,20 @@ +#pragma once + +#include <cstdint> + +extern const uint8_t sensor_start_qvga[8][2]; +extern const uint8_t bridge_start_qvga[9][2]; +extern const uint8_t sensor_start_vga[8][2]; +extern const uint8_t bridge_start_vga[9][2]; +extern const uint8_t ov772x_reg_initdata[35][2]; +extern const uint8_t ov534_reg_initdata[47][2]; + +extern const struct rate_s rate_0[14]; +extern const struct rate_s rate_1[22]; + +struct rate_s { + uint16_t fps; + uint8_t r11; + uint8_t r0d; + uint8_t re5; +}; diff --git a/video-ps3eye/PS3EYEDriver/frame-queue.cpp b/video-ps3eye/PS3EYEDriver/frame-queue.cpp new file mode 100644 index 00000000..00990218 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/frame-queue.cpp @@ -0,0 +1,326 @@ +#include "frame-queue.hpp" +#include "log.hpp" +#include "compat/macros1.h" + +using namespace std::chrono_literals; + +FrameQueue::FrameQueue(uint32_t frame_size) : + frame_buffer(std::make_unique<uint8_t[]>(frame_size * num_frames)), + frame_size(frame_size) +{ +} + +uint8_t* FrameQueue::Enqueue() +{ + std::lock_guard<std::mutex> lock(mutex); + + uint8_t* new_frame; + + // Unlike traditional producer/consumer, we don't block the producer if the buffer is full (ie. the consumer is not reading data fast enough). + // Instead, if the buffer is full, we simply return the current frame pointer, causing the producer to overwrite the previous frame. + // This allows performance to degrade gracefully: if the consumer is not fast enough (< Camera FPS), it will miss frames, but if it is fast enough (>= Camera FPS), it will see everything. + // + // Note that because the the producer is writing directly to the ring buffer, we can only ever be a maximum of num_frames-1 ahead of the consumer, + // otherwise the producer could overwrite the frame the consumer is currently reading (in case of a slow consumer) + if (available >= num_frames - 1) + return ptr() + head * frame_size; + + // Note: we don't need to copy any data to the buffer since the USB packets are directly written to the frame buffer. + // We just need to update head and available count to signal to the consumer that a new frame is available + head = (head + 1) % num_frames; + available++; + + // Determine the next frame pointer that the producer should write to + new_frame = ptr() + head * frame_size; + + // Signal consumer that data became available + queue_cvar.notify_one(); + + return new_frame; +} + +bool FrameQueue::Dequeue(uint8_t* dest, int width, int height, ps3eye_camera::format fmt, bool flip_v) +{ + std::unique_lock<std::mutex> lock(mutex); + + if (!queue_cvar.wait_for(lock, 3000ms, [this] { return available != 0; })) + { + debug2("frame timeout\n"); + return false; + } + + // Copy from internal buffer + uint8_t* src = ptr() + frame_size * tail; + + using f = typename ps3eye_camera::format; + + switch (fmt) + { + case f::format_Bayer: + memcpy(dest, src, frame_size); + break; + case f::format_BGR: + case f::format_RGB: + debayer_RGB<3>(width, height, src, dest, fmt == f::format_BGR, flip_v); + break; + case f::format_BGRA: + case f::format_RGBA: + debayer_RGB<4>(width, height, src, dest, fmt == ps3eye_camera::format_BGRA, flip_v); + break; + case f::format_Gray: + DebayerGray(width, height, src, dest); + break; + default: + unreachable(); + } + + // Update tail and available count + tail = (tail + 1) % num_frames; + available--; + + return true; +} + +void FrameQueue::DebayerGray(int frame_width, int frame_height, const uint8_t* inBayer, uint8_t* outBuffer) +{ + // PSMove output is in the following Bayer format (GRBG): + // + // G R G R G R + // B G B G B G + // G R G R G R + // B G B G B G + // + // This is the normal Bayer pattern shifted left one place. + + int source_stride = frame_width; + const uint8_t* source_row = inBayer; // Start at first bayer pixel + int dest_stride = frame_width; + uint8_t* dest_row = outBuffer + dest_stride + 1; // We start outputting at the second pixel of the second row's G component + uint32_t R,G,B; + + // Fill rows 1 to height-2 of the destination buffer. First and last row are filled separately (they are copied from the second row and second-to-last rows respectively) + for (int y = 0; y < frame_height-2; source_row += source_stride, dest_row += dest_stride, ++y) + { + const uint8_t* source = source_row; + const uint8_t* source_end = source + (source_stride-2); // -2 to deal with the fact that we're starting at the second pixel of the row and should end at the second-to-last pixel of the row (first and last are filled separately) + uint8_t* dest = dest_row; + + // Row starting with Green + if (y % 2 == 0) + { + // Fill first pixel (green) + B = (uint32_t) ((source[source_stride] + source[source_stride + 2] + 1) >> 1); + G = source[source_stride + 1]; + R = (uint32_t) ((source[1] + source[source_stride * 2 + 1] + 1) >> 1); + *dest = (uint8_t)((R*77 + G*151 + B*28)>>8); + + source++; + dest++; + + // Fill remaining pixel + for (; source <= source_end - 2; source += 2, dest += 2) + { + // Blue pixel + B = source[source_stride + 1]; + G = (uint32_t) ((source[1] + source[source_stride] + source[source_stride + 2] + source[source_stride * 2 + 1] + 2) >> 2); + R = (uint32_t) ((source[0] + source[2] + source[source_stride * 2] + source[source_stride * 2 + 2] + 2) >> 2); + dest[0] = (uint8_t)((R*77 + G*151 + B*28)>>8); + + // Green pixel + B = (uint32_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1); + G = source[source_stride + 2]; + R = (uint32_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1); + dest[1] = (uint8_t)((R*77 + G*151 + B*28)>>8); + + } + } + else + { + for (; source <= source_end - 2; source += 2, dest += 2) + { + // Red pixel + B = (uint32_t) ((source[0] + source[2] + source[source_stride * 2] + source[source_stride * 2 + 2] + 2) >> 2);; + G = (uint32_t) ((source[1] + source[source_stride] + source[source_stride + 2] + source[source_stride * 2 + 1] + 2) >> 2);; + R = source[source_stride + 1]; + dest[0] = (uint8_t)((R*77 + G*151 + B*28)>>8); + + // Green pixel + B = (uint32_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1); + G = source[source_stride + 2]; + R = (uint32_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1); + dest[1] = (uint8_t)((R*77 + G*151 + B*28)>>8); + } + } + + if (source < source_end) + { + B = source[source_stride + 1]; + G = (uint32_t) ((source[1] + source[source_stride] + source[source_stride + 2] + source[source_stride * 2 + 1] + 2) >> 2); + R = (uint32_t) ((source[0] + source[2] + source[source_stride * 2] + source[source_stride * 2 + 2] + 2) >> 2);; + dest[0] = (uint8_t)((R*77 + G*151 + B*28)>>8); + + source++; + dest++; + } + + // Fill first pixel of row (copy second pixel) + uint8_t* first_pixel = dest_row-1; + first_pixel[0] = dest_row[0]; + + // Fill last pixel of row (copy second-to-last pixel). Note: dest row starts at the *second* pixel of the row, so dest_row + (width-2) * num_output_channels puts us at the last pixel of the row + uint8_t* last_pixel = dest_row + (frame_width - 2); + uint8_t* second_to_last_pixel = last_pixel - 1; + last_pixel[0] = second_to_last_pixel[0]; + } + + // Fill first & last row + for (int i = 0; i < dest_stride; i++) + { + outBuffer[i] = outBuffer[i + dest_stride]; + outBuffer[i + (frame_height - 1)*dest_stride] = outBuffer[i + (frame_height - 2)*dest_stride]; + } +} + +template<int nchannels> +void FrameQueue::debayer_RGB(int frame_width, int frame_height, const uint8_t* inBayer, uint8_t* outBuffer, bool inBGR, bool flip_v) +{ + // PSMove output is in the following Bayer format (GRBG): + // + // G R G R G R + // B G B G B G + // G R G R G R + // B G B G B G + // + // This is the normal Bayer pattern shifted left one place. + + int source_stride = frame_width; + int dest_stride = frame_width * nchannels; + // Start at first bayer pixel + const uint8_t* source_row = inBayer; + // We start outputting at the second pixel of the second row's G component + uint8_t* dest_row = outBuffer + dest_stride + nchannels + 1; + int swap_br = inBGR ? 1 : -1; + + // Fill rows 1 to height-2 of the destination buffer. First and last row are filled separately (they are copied from the second row and second-to-last rows respectively) + for (int y = 0; y < frame_height-2; source_row += source_stride, dest_row += dest_stride, ++y) + { + const uint8_t* source = source_row; + // -2 to deal with the fact that we're starting at the second pixel of the row and should end at the second-to-last pixel of the row (first and last are filled separately) + const uint8_t* source_end = source + (source_stride-2); + uint8_t* dest = dest_row; + + // Row starting with Green + if (y % 2 == (int)flip_v) + { + // Fill first pixel (green) + dest[-1*swap_br] = (uint8_t) ((source[source_stride] + source[source_stride + 2] + 1) >> 1); + dest[0] = source[source_stride + 1]; + dest[1*swap_br] = (uint8_t) ((source[1] + source[source_stride * 2 + 1] + 1) >> 1); + set_alpha<nchannels>(dest); + + source++; + dest += nchannels; + + // Fill remaining pixel + for (; source <= source_end - 2; source += 2, dest += nchannels * 2) + { + // Blue pixel + uint8_t* cur_pixel = dest; + cur_pixel[-1*swap_br] = source[source_stride + 1]; + cur_pixel[0] = (uint8_t) ((source[1] + + source[source_stride] + + source[source_stride + 2] + + source[source_stride * 2 + 1] + + 2) >> 2); + cur_pixel[1*swap_br] = (uint8_t) ((source[0] + + source[2] + + source[source_stride * 2] + + source[source_stride * 2 + 2] + + 2) >> 2); + set_alpha<nchannels>(cur_pixel); + + // Green pixel + uint8_t* next_pixel = cur_pixel+nchannels; + next_pixel[-1*swap_br] = (uint8_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1); + next_pixel[0] = source[source_stride + 2]; + next_pixel[1*swap_br] = (uint8_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1); + set_alpha<nchannels>(next_pixel); + } + } + else + { + for (; source <= source_end - 2; source += 2, dest += nchannels * 2) + { + // Red pixel + uint8_t* cur_pixel = dest; + cur_pixel[-1*swap_br] = (uint8_t) ((source[0] + + source[2] + + source[source_stride * 2] + + source[source_stride * 2 + 2] + + 2) >> 2);; + cur_pixel[0] = (uint8_t) ((source[1] + + source[source_stride] + + source[source_stride + 2] + + source[source_stride * 2 + 1] + + 2) >> 2);; + cur_pixel[1*swap_br] = source[source_stride + 1]; + set_alpha<nchannels>(cur_pixel); + + // Green pixel + uint8_t* next_pixel = cur_pixel+nchannels; + next_pixel[-1*swap_br] = (uint8_t) ((source[2] + source[source_stride * 2 + 2] + 1) >> 1); + next_pixel[0] = source[source_stride + 2]; + next_pixel[1*swap_br] = (uint8_t) ((source[source_stride + 1] + source[source_stride + 3] + 1) >> 1); + set_alpha<nchannels>(next_pixel); + } + } + + if (source < source_end) + { + dest[-1*swap_br] = source[source_stride + 1]; + dest[0] = (uint8_t) ((source[1] + + source[source_stride] + + source[source_stride + 2] + + source[source_stride * 2 + 1] + + 2) >> 2); + dest[1*swap_br] = (uint8_t) ((source[0] + + source[2] + + source[source_stride * 2] + + source[source_stride * 2 + 2] + + 2) >> 2); + set_alpha<nchannels>(dest); + + source++; + dest += nchannels; + } + + // Fill first pixel of row (copy second pixel) + uint8_t* first_pixel = dest_row-nchannels; + first_pixel[-1*swap_br] = dest_row[-1*swap_br]; + first_pixel[0] = dest_row[0]; + first_pixel[1*swap_br] = dest_row[1*swap_br]; + set_alpha<nchannels>(first_pixel); + + // Fill last pixel of row (copy second-to-last pixel). Note: dest row starts at the *second* pixel of the row, so dest_row + (width-2) * nchannels puts us at the last pixel of the row + uint8_t* last_pixel = dest_row + (frame_width - 2)*nchannels; + uint8_t* second_to_last_pixel = last_pixel - nchannels; + + last_pixel[-1*swap_br] = second_to_last_pixel[-1*swap_br]; + last_pixel[0] = second_to_last_pixel[0]; + last_pixel[1*swap_br] = second_to_last_pixel[1*swap_br]; + set_alpha<nchannels>(last_pixel); + } + + // Fill first & last row + for (int i = 0; i < dest_stride; i++) + { + outBuffer[i] = outBuffer[i + dest_stride]; + outBuffer[i + (frame_height - 1)*dest_stride] = outBuffer[i + (frame_height - 2)*dest_stride]; + } +} + +template<> void FrameQueue::set_alpha<3>(uint8_t*) {} +template<> void FrameQueue::set_alpha<4>(uint8_t* destGreen) { destGreen[2] = 255; } + +template void FrameQueue::debayer_RGB<3>(int, int, const uint8_t*, uint8_t*, bool, bool); +template void FrameQueue::debayer_RGB<4>(int, int, const uint8_t*, uint8_t*, bool, bool); diff --git a/video-ps3eye/PS3EYEDriver/frame-queue.hpp b/video-ps3eye/PS3EYEDriver/frame-queue.hpp new file mode 100644 index 00000000..8cee2223 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/frame-queue.hpp @@ -0,0 +1,33 @@ +#pragma once +#include "ps3eye.hpp" + +#include <mutex> +#include <condition_variable> + +struct FrameQueue final +{ + FrameQueue(uint32_t frame_size); + + inline uint8_t* ptr() { return frame_buffer.get(); } + uint8_t* Enqueue(); + + bool Dequeue(uint8_t* dest, int width, int height, ps3eye_camera::format fmt, bool flip_v); + static void DebayerGray(int frame_width, int frame_height, const uint8_t* inBayer, uint8_t* outBuffer); + + template<int nchannels> + static void set_alpha(uint8_t* destGreen); + + template<int nchannels> + void debayer_RGB(int frame_width, int frame_height, const uint8_t* inBayer, uint8_t* outBuffer, bool inBGR, bool flip_v); + +private: + std::unique_ptr<uint8_t[]> frame_buffer; + std::mutex mutex; + std::condition_variable queue_cvar; + uint32_t frame_size = 0; + uint32_t head = 0; + uint32_t tail = 0; + uint32_t available = 0; + + static constexpr unsigned num_frames = 4; +}; diff --git a/video-ps3eye/PS3EYEDriver/internal.hpp b/video-ps3eye/PS3EYEDriver/internal.hpp new file mode 100644 index 00000000..310251a5 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/internal.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include "log.hpp" + +#if defined _MSC_VER && 0 +// Get rid of annoying zero length structure warnings from libusb.h in MSVC +# pragma warning(push) +# pragma warning(disable : 4200) +#endif + +#include "libusb.h" + +#if defined _MSC_VER && 0 +# pragma warning(pop) +#endif + diff --git a/video-ps3eye/PS3EYEDriver/log.hpp b/video-ps3eye/PS3EYEDriver/log.hpp new file mode 100644 index 00000000..6ae50028 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/log.hpp @@ -0,0 +1,26 @@ +#pragma once + +#include <cstdio> + +template<unsigned N, unsigned M, typename... xs> +void ps3eye_log(const char (&prefix)[N], const char (&fmt)[M], const xs&... args) +{ + fprintf(stderr, "%s ", prefix); + fprintf(stderr, fmt, args...); + if constexpr(M > 1) + if (fmt[M-2] != '\n') + fprintf(stderr, "\n"); + fflush(stderr); +} + +#define warn(...) ps3eye_log("[ps3eye warn]", __VA_ARGS__) + +#define PS3_EYE_DEBUG + +#ifdef PS3_EYE_DEBUG +# define debug(...) ps3eye_log("[ps3eye debug]", __VA_ARGS__) +# define debug2(...) ps3eye_log("[ps3eye debug2]", __VA_ARGS__) +#else +# define debug(...) ((void)0) +# define debug2(...) ((void)0) +#endif diff --git a/video-ps3eye/PS3EYEDriver/ps3eye.cpp b/video-ps3eye/PS3EYEDriver/ps3eye.cpp new file mode 100644 index 00000000..010ab12c --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/ps3eye.cpp @@ -0,0 +1,573 @@ +#include "ps3eye.hpp" +#include "compat/macros1.h" +#include "compat/thread-name.hpp" +#include "internal.hpp" + +#include "urb.hpp" +#include "frame-queue.hpp" +#include "singleton.hpp" +#include "constants.hpp" + +#include <memory> +#include <thread> +#include <mutex> +#include <condition_variable> +#include <chrono> +#include <atomic> +#include <optional> +#include <iterator> +#include <algorithm> + +#include <cstdio> +#include <cstdlib> +#include <cstring> +#include <cstdint> + +using namespace std::chrono_literals; + +#define OV534_REG_ADDRESS 0xf1 /* sensor address */ +#define OV534_REG_SUBADDR 0xf2 +#define OV534_REG_WRITE 0xf3 +#define OV534_REG_READ 0xf4 +#define OV534_REG_OPERATION 0xf5 +#define OV534_REG_STATUS 0xf6 + +#define OV534_OP_WRITE_3 0x37 +#define OV534_OP_WRITE_2 0x33 +#define OV534_OP_READ_2 0xf9 + +bool ps3eye_camera::devicesEnumerated = false; +std::vector<ps3eye_camera::device> ps3eye_camera::devices; + +const std::vector<ps3eye_camera::device>& ps3eye_camera::get_devices(bool force_refresh) +{ + if (devicesEnumerated && !force_refresh) + return devices; + + devices.clear(); + + (void) USBMgr::instance().list_devices(devices); + + devicesEnumerated = true; + return devices; +} + +ps3eye_camera::ps3eye_camera(libusb_device *device) : + device_(device), + urb(std::make_shared<URBDesc>()) +{ +} + +ps3eye_camera::~ps3eye_camera() +{ + release(); +} + +void ps3eye_camera::release() +{ + stop(); + close_usb(); +} + +bool ps3eye_camera::init(uint32_t width, uint32_t height, uint16_t desiredFrameRate, format outputFormat) +{ + uint16_t sensor_id; + + // open usb device so we can setup and go + if(handle_ == nullptr && !open_usb()) + return false; + + // find best cam mode + if((width == 0 && height == 0) || width > 320 || height > 240) + { + frame_width = 640; + frame_height = 480; + } else { + frame_width = 320; + frame_height = 240; + } + frame_rate = ov534_set_frame_rate(desiredFrameRate, true); + frame_output_format = outputFormat; + + /* reset bridge */ + ov534_reg_write(0xe7, 0x3a); + ov534_reg_write(0xe0, 0x08); + + std::this_thread::sleep_for(100ms); + + /* initialize the sensor address */ + ov534_reg_write(OV534_REG_ADDRESS, 0x42); + + /* reset sensor */ + sccb_reg_write(0x12, 0x80); + + std::this_thread::sleep_for(100ms); + + /* probe the sensor */ + sccb_reg_read(0x0a); + sensor_id = (uint16_t)(sccb_reg_read(0x0a) << 8); + sccb_reg_read(0x0b); + sensor_id |= sccb_reg_read(0x0b); + debug("sensor id: %04x\n", sensor_id); + + /* initialize */ + reg_w_array(ov534_reg_initdata, std::size(ov534_reg_initdata)); + ov534_set_led(1); + sccb_w_array(ov772x_reg_initdata, std::size(ov772x_reg_initdata)); + ov534_reg_write(0xe0, 0x09); + ov534_set_led(0); + + return true; +} + +bool ps3eye_camera::start() +{ + if (is_streaming_) + return true; + + if (!isInitialized()) + { + debug("call ps3eye_camera::init() first\n"); + return false; + } + + if (frame_width == 320) { /* 320x240 */ + reg_w_array(bridge_start_qvga, std::size(bridge_start_qvga)); + sccb_w_array(sensor_start_qvga, std::size(sensor_start_qvga)); + } else { /* 640x480 */ + reg_w_array(bridge_start_vga, std::size(bridge_start_vga)); + sccb_w_array(sensor_start_vga, std::size(sensor_start_vga)); + } + + ov534_set_frame_rate(frame_rate); + + set_auto_gain(autogain); + set_auto_white_balance(awb); + set_gain(gain); + set_hue(hue); + set_exposure(exposure); + set_brightness(brightness); + set_contrast(contrast); + set_sharpness(sharpness); + set_red_balance(redblc); + set_blue_balance(blueblc); + set_green_balance(greenblc); + set_flip(flip_h, flip_v); + + ov534_set_led(1); + ov534_reg_write(0xe0, 0x00); // start stream + + is_streaming_ = true; + // init and start urb + if (!urb->start_transfers(handle_, frame_width*frame_height)) + { + debug("failed to start\n"); + stop(); + return false; + } + return true; +} + +void ps3eye_camera::stop() +{ + if (!is_streaming_) + return; + + /* stop streaming data */ + ov534_reg_write(0xe0, 0x09); + ov534_set_led(0); + + // close urb + urb->close_transfers(); + urb->free_transfers(); + + is_streaming_ = false; +} + +#define MAX_USB_DEVICE_PORT_PATH 7 + +bool ps3eye_camera::getUSBPortPath(char *out_identifier, size_t max_identifier_length) const +{ + bool success = false; + + if (isInitialized()) + { + uint8_t port_numbers[MAX_USB_DEVICE_PORT_PATH]; + + memset(out_identifier, 0, max_identifier_length); + memset(port_numbers, 0, std::size(port_numbers)); + + int port_count = libusb_get_port_numbers(device_, port_numbers, MAX_USB_DEVICE_PORT_PATH); + int bus_id = libusb_get_bus_number(device_); + + snprintf(out_identifier, max_identifier_length, "b%d", bus_id); + if (port_count > 0) + { + success = true; + + for (int port_index = 0; port_index < port_count; ++port_index) + { + uint8_t port_number = port_numbers[port_index]; + char port_string[8]; + + snprintf(port_string, std::size(port_string), (port_index == 0) ? "_p%d" : ".%d", port_number); + + if (strlen(out_identifier)+std::size(port_string)+1 <= max_identifier_length) + std::strcat(out_identifier, port_string); + else + { + success = false; + break; + } + } + } + } + + return success; +} + +uint32_t ps3eye_camera::get_depth() const +{ + switch (frame_output_format) + { + case format_Bayer: + case format_Gray: + return 1; + case format_BGR: + case format_RGB: + return 3; + case format_BGRA: + case format_RGBA: + return 4; + default: + unreachable(); + } +} + +bool ps3eye_camera::get_frame(uint8_t* frame) +{ + return urb->queue().Dequeue(frame, frame_width, frame_height, frame_output_format, flip_v); +} + +bool ps3eye_camera::open_usb() +{ + // open, set first config and claim interface + int res = libusb_open(device_, &handle_); + if(res != 0) { + debug("device open error: %d\n", res); + return false; + } + + res = libusb_claim_interface(handle_, 0); + if(res != 0) { + debug("device claim interface error: %d\n", res); + return false; + } + + return true; +} + +void ps3eye_camera::close_usb() +{ + if (handle_) + { + libusb_release_interface(handle_, 0); + libusb_close(handle_); + } + if (device_) + libusb_unref_device(device_); + + handle_ = nullptr; + device_ = nullptr; +} + +/* Two bits control LED: 0x21 bit 7 and 0x23 bit 7. + * (direction and output)? */ +void ps3eye_camera::ov534_set_led(int status) +{ + uint8_t data; + + debug("led status: %d\n", status); + + data = ov534_reg_read(0x21); + data |= 0x80; + ov534_reg_write(0x21, data); + + data = ov534_reg_read(0x23); + if (status) + data |= 0x80; + else + data &= ~0x80; + + ov534_reg_write(0x23, data); + + if (!status) { + data = ov534_reg_read(0x21); + data &= ~0x80; + ov534_reg_write(0x21, data); + } +} + +/* validate frame rate and (if not dry run) set it */ +uint16_t ps3eye_camera::ov534_set_frame_rate(uint16_t fps, bool dry_run) +{ + const struct rate_s *r; + int i; + + if (frame_width == 640) { + r = rate_0; + i = std::size(rate_0); + } else { + r = rate_1; + i = std::size(rate_1); + } + while (--i > 0) { + if (fps >= r->fps) + break; + r++; + } + + if (!dry_run) { + sccb_reg_write(0x11, r->r11); + sccb_reg_write(0x0d, r->r0d); + ov534_reg_write(0xe5, r->re5); + } + + debug("frame_rate: %d\n", r->fps); + return r->fps; +} + +void ps3eye_camera::ov534_reg_write(uint16_t reg, uint8_t val) +{ + int ret; + + //debug("reg=0x%04x, val=0%02x", reg, val); + usb_buf[0] = val; + + constexpr int req_type = (int)LIBUSB_ENDPOINT_OUT | + (int)LIBUSB_REQUEST_TYPE_VENDOR | + (int)LIBUSB_RECIPIENT_DEVICE; + + ret = libusb_control_transfer(handle_, + req_type, + 0x01, 0x00, reg, + usb_buf, 1, 500); + if (ret < 0) { + debug("write failed\n"); + } +} + +uint8_t ps3eye_camera::ov534_reg_read(uint16_t reg) +{ + int ret; + + constexpr int req_type = (int)LIBUSB_ENDPOINT_IN | + (int)LIBUSB_REQUEST_TYPE_VENDOR | + (int)LIBUSB_RECIPIENT_DEVICE; + + ret = libusb_control_transfer(handle_, req_type, 0x01, 0x00, reg, usb_buf, 1, 500); + + //debug("reg=0x%04x, data=0x%02x", reg, usb_buf[0]); + if (ret < 0) + warn("read failed\n"); + return usb_buf[0]; +} + +int ps3eye_camera::sccb_check_status() +{ + uint8_t data; + int i; + + for (i = 0; i < 5; i++) { + data = ov534_reg_read(OV534_REG_STATUS); + + switch (data) { + case 0x00: + return 1; + case 0x04: + return 0; + case 0x03: + break; + default: + debug("sccb status 0x%02x, attempt %d/5\n", + data, i + 1); + } + } + return 0; +} + +void ps3eye_camera::sccb_reg_write(uint8_t reg, uint8_t val) +{ + //debug("reg: 0x%02x, val: 0x%02x", reg, val); + ov534_reg_write(OV534_REG_SUBADDR, reg); + ov534_reg_write(OV534_REG_WRITE, val); + ov534_reg_write(OV534_REG_OPERATION, OV534_OP_WRITE_3); + + if (!sccb_check_status()) { + debug("sccb_reg_write failed\n"); + } +} + + +uint8_t ps3eye_camera::sccb_reg_read(uint16_t reg) +{ + ov534_reg_write(OV534_REG_SUBADDR, (uint8_t)reg); + ov534_reg_write(OV534_REG_OPERATION, OV534_OP_WRITE_2); + if (!sccb_check_status()) { + debug("sccb_reg_read failed 1\n"); + } + + ov534_reg_write(OV534_REG_OPERATION, OV534_OP_READ_2); + if (!sccb_check_status()) { + debug( "sccb_reg_read failed 2\n"); + } + + return ov534_reg_read(OV534_REG_READ); +} +/* output a bridge sequence (reg - val) */ +void ps3eye_camera::reg_w_array(const uint8_t (*data)[2], int len) +{ + while (--len >= 0) { + ov534_reg_write((*data)[0], (*data)[1]); + data++; + } +} + +/* output a sensor sequence (reg - val) */ +void ps3eye_camera::sccb_w_array(const uint8_t (*data)[2], int len) +{ + while (--len >= 0) { + if ((*data)[0] != 0xff) { + sccb_reg_write((*data)[0], (*data)[1]); + } else { + sccb_reg_read((*data)[1]); + sccb_reg_write(0xff, 0x00); + } + data++; + } +} + +void ps3eye_camera::set_auto_white_balance(bool val) +{ + awb = val; + if (val) { + sccb_reg_write(0x63, 0xe0); //AWB ON + }else{ + sccb_reg_write(0x63, 0xAA); //AWB OFF + } +} + +void ps3eye_camera::set_gain(uint8_t val) +{ + gain = val; + switch(val & 0x30){ + case 0x00: + val &=0x0F; + break; + case 0x10: + val &=0x0F; + val |=0x30; + break; + case 0x20: + val &=0x0F; + val |=0x70; + break; + case 0x30: + val &=0x0F; + val |=0xF0; + break; + } + sccb_reg_write(0x00, val); +} + +void ps3eye_camera::set_exposure(uint8_t val) +{ + exposure = val; + sccb_reg_write(0x08, val>>7); + sccb_reg_write(0x10, val<<1); +} + +void ps3eye_camera::set_sharpness(uint8_t val) +{ + sharpness = val; + sccb_reg_write(0x91, val); //vga noise + sccb_reg_write(0x8E, val); //qvga noise +} + +void ps3eye_camera::set_contrast(uint8_t val) +{ + contrast = val; + sccb_reg_write(0x9C, val); +} + +void ps3eye_camera::set_brightness(uint8_t val) +{ + brightness = val; + sccb_reg_write(0x9B, val); +} + +void ps3eye_camera::set_hue(uint8_t val) +{ + hue = val; + sccb_reg_write(0x01, val); +} + +void ps3eye_camera::set_red_balance(uint8_t val) +{ + redblc = val; + sccb_reg_write(0x43, val); +} + +void ps3eye_camera::set_blue_balance(uint8_t val) +{ + blueblc = val; + sccb_reg_write(0x42, val); +} + +void ps3eye_camera::set_green_balance(uint8_t val) +{ + greenblc = val; + sccb_reg_write(0x44, val); +} + +void ps3eye_camera::set_flip(bool horizontal, bool vertical) +{ + flip_h = horizontal; + flip_v = vertical; + uint8_t val = sccb_reg_read(0x0c); + val &= ~0xc0; + if (!horizontal) val |= 0x40; + if (!vertical) val |= 0x80; + sccb_reg_write(0x0c, val); +} + +void ps3eye_camera::set_test_pattern(bool enable) +{ + testPattern = enable; + uint8_t val = sccb_reg_read(0x0C); + val &= ~0b00000001; + if (testPattern) val |= 0b00000001; // 0x80; + sccb_reg_write(0x0C, val); +} + +bool ps3eye_camera::setFrameRate(uint8_t val) +{ + if (is_streaming_) return false; + frame_rate = ov534_set_frame_rate(val, true); + return true; +} + +void ps3eye_camera::set_auto_gain(bool val) +{ + autogain = val; + if (val) { + sccb_reg_write(0x13, 0xf7); //AGC,AEC,AWB ON + sccb_reg_write(0x64, sccb_reg_read(0x64)|0x03); + } else { + sccb_reg_write(0x13, 0xf0); //AGC,AEC,AWB OFF + sccb_reg_write(0x64, sccb_reg_read(0x64)&0xFC); + + set_gain(gain); + set_exposure(exposure); + } +} diff --git a/video-ps3eye/PS3EYEDriver/ps3eye.hpp b/video-ps3eye/PS3EYEDriver/ps3eye.hpp new file mode 100644 index 00000000..fea71fe3 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/ps3eye.hpp @@ -0,0 +1,139 @@ +#pragma once + +#include <vector> +#include <memory> +#include <cinttypes> + +struct libusb_device; +struct libusb_device_handle; + +struct ps3eye_camera final +{ + enum class format + { + format_Bayer, // Output in Bayer. Destination buffer must be width * height bytes + format_BGR, // Output in BGR. Destination buffer must be width * height * 3 bytes + format_RGB, // Output in RGB. Destination buffer must be width * height * 3 bytes + format_BGRA, // Output in BGRA. Destination buffer must be width * height * 4 bytes + format_RGBA, // Output in RGBA. Destination buffer must be width * height * 4 bytes + format_Gray // Output in Grayscale. Destination buffer must be width * height bytes + }; + + static constexpr format format_Bayer = format::format_Bayer; + static constexpr format format_BGR = format::format_BGR; + static constexpr format format_RGB = format::format_RGB; + static constexpr format format_BGRA = format::format_BGRA; + static constexpr format format_RGBA = format::format_RGBA; + static constexpr format format_Gray = format::format_Gray; + + using device = std::shared_ptr<ps3eye_camera>; + + static constexpr uint16_t VENDOR_ID = 0x1415; + static constexpr uint16_t PRODUCT_ID = 0x2000; + + ps3eye_camera(libusb_device *device); + ~ps3eye_camera(); + ps3eye_camera(const ps3eye_camera&) = delete; + void operator=(const ps3eye_camera&) = delete; + + bool init(uint32_t width = 0, uint32_t height = 0, uint16_t desiredFrameRate = 30, format outputFormat = format_BGR); + [[nodiscard]] bool start(); + void stop(); + + // Controls + + bool get_auto_gain() const { return autogain; } + void set_auto_gain(bool val); + bool get_auto_white_balance() const { return awb; } + void set_auto_white_balance(bool val); + uint8_t get_gain() const { return gain; } + void set_gain(uint8_t val); + uint8_t get_exposure() const { return exposure; } + void set_exposure(uint8_t val); + uint8_t getSharpness() const { return sharpness; } + void set_sharpness(uint8_t val); + uint8_t get_contrast() const { return contrast; } + void set_contrast(uint8_t val); + uint8_t get_brightness() const { return brightness; } + void set_brightness(uint8_t val); + uint8_t get_hue() const { return hue; } + void set_hue(uint8_t val); + uint8_t get_red_balance() const { return redblc; } + void set_red_balance(uint8_t val); + uint8_t get_blue_balance() const { return blueblc; } + void set_blue_balance(uint8_t val); + uint8_t get_green_balance() const { return greenblc; } + void set_green_balance(uint8_t val); + bool get_flip_h() const { return flip_h; } + bool get_flip_v() const { return flip_v; } + void set_flip(bool horizontal = false, bool vertical = false); + bool get_test_pattern() const { return testPattern; } + void set_test_pattern(bool enable); + bool isStreaming() const { return is_streaming_; } + bool isInitialized() const { return device_ && handle_; } + bool getUSBPortPath(char *out_identifier, size_t max_identifier_length) const; + + // Get a frame from the camera. Notes: + // - If there is no frame available, this function will block until one is + // - The output buffer must be sized correctly, depending out the output format. See EOutputFormat. + [[nodiscard]] bool get_frame(uint8_t* frame); + + uint32_t getWidth() const { return frame_width; } + uint32_t getHeight() const { return frame_height; } + uint16_t getFrameRate() const { return frame_rate; } + bool setFrameRate(uint8_t val); + uint32_t get_stride() const { return frame_width * get_depth(); } + uint32_t get_depth() const; + + static const std::vector<device>& get_devices(bool force_refresh = false); + +private: + // usb ops + uint16_t ov534_set_frame_rate(uint16_t frame_rate, bool dry_run = false); + void ov534_set_led(int status); + void ov534_reg_write(uint16_t reg, uint8_t val); + uint8_t ov534_reg_read(uint16_t reg); + int sccb_check_status(); + void sccb_reg_write(uint8_t reg, uint8_t val); + uint8_t sccb_reg_read(uint16_t reg); + void reg_w_array(const uint8_t (*data)[2], int len); + void sccb_w_array(const uint8_t (*data)[2], int len); + + // controls + uint8_t gain = 63; // 0 <-> 63 + uint8_t exposure = 255; // 0 <-> 255 + uint8_t hue = 143; // 0 <-> 255 + uint8_t brightness = 20; // 0 <-> 255 + uint8_t contrast = 37; // 0 <-> 255 + uint8_t blueblc = 128; // 0 <-> 255 + uint8_t redblc = 128; // 0 <-> 255 + uint8_t greenblc = 128; // 0 <-> 255 + uint8_t sharpness = 0; // 0 <-> 63 + + static bool devicesEnumerated; + static std::vector<device> devices; + + uint32_t frame_width = 0; + uint32_t frame_height = 0; + uint16_t frame_rate = 0; + format frame_output_format = format_Bayer; + + //usb stuff + libusb_device *device_ = nullptr; + libusb_device_handle *handle_ = nullptr; + uint8_t usb_buf[64] = {}; + + std::shared_ptr<struct URBDesc> urb = nullptr; + + bool awb = false; + bool flip_h = false; + bool flip_v = false; + bool testPattern = false; + bool autogain = false; + bool is_streaming_ = false; + + void release(); + bool open_usb(); + void close_usb(); + +}; diff --git a/video-ps3eye/PS3EYEDriver/sdl/CMakeLists.txt b/video-ps3eye/PS3EYEDriver/sdl/CMakeLists.txt new file mode 100644 index 00000000..5064b2d8 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/sdl/CMakeLists.txt @@ -0,0 +1,24 @@ +set(cmake-modules "${CMAKE_CURRENT_LIST_DIR}/cmake/") +if(MSVC) + set(CMAKE_MODULE_PATH "${cmake-modules}" ${CMAKE_MODULE_PATH}) +else() + list(APPEND CMAKE_MODULE_PATH "${cmake-modules}") +endif() + +find_package(SDL2 QUIET) +if(SDL2_FOUND) + include_directories(${SDL2_INCLUDE_DIRS}) + link_libraries(${SDL2_LIBRARIES}) + link_libraries(opentrack-ps3eye) + otr_module(ps3eye-test EXECUTABLE NO-QT WIN32-CONSOLE) + if(WIN32) + foreach(k ${SDL2_LIBRARIES}) + get_filename_component(path "${k}" PATH) + set(lib "${path}/SDL2.dll") + if(EXISTS "${lib}") + otr_install_lib("${lib}" "${opentrack-hier-pfx}") + break() + endif() + endforeach() + endif() +endif() diff --git a/video-ps3eye/PS3EYEDriver/sdl/cmake/FindSDL2.cmake b/video-ps3eye/PS3EYEDriver/sdl/cmake/FindSDL2.cmake index 185d6e9e..e55e5849 100644 --- a/video-ps3eye/PS3EYEDriver/sdl/cmake/FindSDL2.cmake +++ b/video-ps3eye/PS3EYEDriver/sdl/cmake/FindSDL2.cmake @@ -1,8 +1,8 @@ # This module defines -# SDL2_LIBRARY, the name of the library to link against +# SDL2_LIBRARIES, the name of the library to link against # SDL2_FOUND, if false, do not try to link to SDL2 -# SDL2_INCLUDE_DIR, where to find SDL.h +# SDL2_INCLUDE_DIRS, where to find SDL.h # # This module responds to the the flag: # SDL2_BUILDING_LIBRARY @@ -76,12 +76,10 @@ SET(SDL2_SEARCH_PATHS /opt/local # DarwinPorts /opt/csw # Blastwave /opt - ${SDL2_PATH} + ${SDL2_DIR} ) FIND_PATH(SDL2_INCLUDE_DIR SDL.h - HINTS - $ENV{SDL2DIR} PATH_SUFFIXES include/SDL2 include PATHS ${SDL2_SEARCH_PATHS} ) @@ -89,10 +87,10 @@ FIND_PATH(SDL2_INCLUDE_DIR SDL.h if(CMAKE_SIZEOF_VOID_P EQUAL 8) set(PATH_SUFFIXES lib64 lib/x64 lib) else() - set(PATH_SUFFIXES lib/x86 lib) + set(PATH_SUFFIXES lib32 lib/x86 lib) endif() -FIND_LIBRARY(SDL2_LIBRARY_TEMP +FIND_LIBRARY(SDL2_LIBRARY NAMES SDL2 HINTS $ENV{SDL2DIR} @@ -100,44 +98,11 @@ FIND_LIBRARY(SDL2_LIBRARY_TEMP PATHS ${SDL2_SEARCH_PATHS} ) -IF(NOT SDL2_BUILDING_LIBRARY) - IF(NOT ${SDL2_INCLUDE_DIR} MATCHES ".framework") - # Non-OS X framework versions expect you to also dynamically link to - # SDL2main. This is mainly for Windows and OS X. Other (Unix) platforms - # seem to provide SDL2main for compatibility even though they don't - # necessarily need it. - FIND_LIBRARY(SDL2MAIN_LIBRARY - NAMES SDL2main - HINTS - $ENV{SDL2DIR} - PATH_SUFFIXES ${PATH_SUFFIXES} - PATHS ${SDL2_SEARCH_PATHS} - ) - ENDIF(NOT ${SDL2_INCLUDE_DIR} MATCHES ".framework") -ENDIF(NOT SDL2_BUILDING_LIBRARY) - -# SDL2 may require threads on your system. -# The Apple build may not need an explicit flag because one of the -# frameworks may already provide it. -# But for non-OSX systems, I will use the CMake Threads package. -IF(NOT APPLE) - FIND_PACKAGE(Threads) -ENDIF(NOT APPLE) - -# MinGW needs an additional link flag, -mwindows -# It's total link flags should look like -lmingw32 -lSDL2main -lSDL2 -mwindows -IF(MINGW) - SET(MINGW32_LIBRARY mingw32 "-mwindows" CACHE STRING "mwindows for MinGW") -ENDIF(MINGW) - -IF(SDL2_LIBRARY_TEMP) - # For SDL2main - IF(NOT SDL2_BUILDING_LIBRARY) - IF(SDL2MAIN_LIBRARY) - SET(SDL2_LIBRARY_TEMP ${SDL2MAIN_LIBRARY} ${SDL2_LIBRARY_TEMP}) - ENDIF(SDL2MAIN_LIBRARY) - ENDIF(NOT SDL2_BUILDING_LIBRARY) +if(SDL2_LIBRARY) + set(SDL2_LIBRARIES "${SDL2_LIBRARY}") +endif() +IF(SDL2_LIBRARIES) # For OS X, SDL2 uses Cocoa as a backend so it must link to Cocoa. # CMake doesn't display the -framework Cocoa string in the UI even # though it actually is there if I modify a pre-used variable. @@ -145,30 +110,13 @@ IF(SDL2_LIBRARY_TEMP) # So I use a temporary variable until the end so I can set the # "real" variable in one-shot. IF(APPLE) - SET(SDL2_LIBRARY_TEMP ${SDL2_LIBRARY_TEMP} "-framework Cocoa") + list(APPEND SDL2_LIBRARIES "-framework Cocoa") ENDIF(APPLE) +ENDIF() - # For threads, as mentioned Apple doesn't need this. - # In fact, there seems to be a problem if I used the Threads package - # and try using this line, so I'm just skipping it entirely for OS X. - IF(NOT APPLE) - SET(SDL2_LIBRARY_TEMP ${SDL2_LIBRARY_TEMP} ${CMAKE_THREAD_LIBS_INIT}) - ENDIF(NOT APPLE) - - # For MinGW library - IF(MINGW) - SET(SDL2_LIBRARY_TEMP ${MINGW32_LIBRARY} ${SDL2_LIBRARY_TEMP}) - ENDIF(MINGW) - - # Set the final string here so the GUI reflects the final state. - SET(SDL2_LIBRARY ${SDL2_LIBRARY_TEMP} CACHE STRING "Where the SDL2 Library can be found") - # Set the temp variable to INTERNAL so it is not seen in the CMake GUI - SET(SDL2_LIBRARY_TEMP "${SDL2_LIBRARY_TEMP}" CACHE INTERNAL "") -ENDIF(SDL2_LIBRARY_TEMP) - -# message("</FindSDL2.cmake>") +if(SDL2_INCLUDE_DIR) + set(SDL2_INCLUDE_DIRS "${SDL2_INCLUDE_DIR}") +endif() INCLUDE(FindPackageHandleStandardArgs) - -FIND_PACKAGE_HANDLE_STANDARD_ARGS(SDL2 REQUIRED_VARS SDL2_LIBRARY SDL2_INCLUDE_DIR) - +FIND_PACKAGE_HANDLE_STANDARD_ARGS(SDL2 REQUIRED_VARS SDL2_LIBRARIES SDL2_INCLUDE_DIRS) diff --git a/video-ps3eye/PS3EYEDriver/sdl/lang/nl_NL.ts b/video-ps3eye/PS3EYEDriver/sdl/lang/nl_NL.ts new file mode 100644 index 00000000..6401616d --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/sdl/lang/nl_NL.ts @@ -0,0 +1,4 @@ +<?xml version="1.0" encoding="utf-8"?> +<!DOCTYPE TS> +<TS version="2.1"> +</TS> diff --git a/video-ps3eye/PS3EYEDriver/sdl/lang/ru_RU.ts b/video-ps3eye/PS3EYEDriver/sdl/lang/ru_RU.ts new file mode 100644 index 00000000..6401616d --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/sdl/lang/ru_RU.ts @@ -0,0 +1,4 @@ +<?xml version="1.0" encoding="utf-8"?> +<!DOCTYPE TS> +<TS version="2.1"> +</TS> diff --git a/video-ps3eye/PS3EYEDriver/sdl/lang/stub.ts b/video-ps3eye/PS3EYEDriver/sdl/lang/stub.ts new file mode 100644 index 00000000..6401616d --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/sdl/lang/stub.ts @@ -0,0 +1,4 @@ +<?xml version="1.0" encoding="utf-8"?> +<!DOCTYPE TS> +<TS version="2.1"> +</TS> diff --git a/video-ps3eye/PS3EYEDriver/sdl/lang/zh_CN.ts b/video-ps3eye/PS3EYEDriver/sdl/lang/zh_CN.ts new file mode 100644 index 00000000..6401616d --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/sdl/lang/zh_CN.ts @@ -0,0 +1,4 @@ +<?xml version="1.0" encoding="utf-8"?> +<!DOCTYPE TS> +<TS version="2.1"> +</TS> diff --git a/video-ps3eye/PS3EYEDriver/sdl/main.cpp b/video-ps3eye/PS3EYEDriver/sdl/main.cpp new file mode 100644 index 00000000..6a5f3482 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/sdl/main.cpp @@ -0,0 +1,192 @@ +/** + * PS3EYEDriver Simple SDL 2 example, using OpenGL where available. + * Thomas Perl <m@thp.io>; 2014-01-10 + * Joseph Howse <josephhowse@nummist.com>; 2014-12-26 + **/ + +#include "../ps3eye.hpp" +#include "../log.hpp" + +#include <sstream> +#include <iostream> + +#include <SDL.h> +#undef main + +struct ps3eye_context { + ps3eye_context(int width, int height, int fps) + { + if (hasDevices()) + { + eye = devices[0]; + eye->init(width, height, (uint16_t)fps); + } + } + + bool hasDevices() { return !devices.empty(); } + + std::vector<ps3eye_camera::device> devices = ps3eye_camera::get_devices(); + ps3eye_camera::device eye = nullptr; + + bool running = true; + Uint32 last_ticks = 0; + Uint32 last_frames = 0; +}; + +void run_camera(int width, int height, int fps, Uint32 duration) +{ + ps3eye_context ctx(width, height, fps); + if (!ctx.hasDevices()) { + printf("No PS3 Eye ps3eye_camera connected\n"); + return; + } + ctx.eye->set_flip(true); /* mirrored left-right */ + + char title[256]; + sprintf(title, "%dx%d@%d\n", ctx.eye->getWidth(), ctx.eye->getHeight(), ctx.eye->getFrameRate()); + + SDL_Window *window = SDL_CreateWindow( + title, SDL_WINDOWPOS_UNDEFINED, + SDL_WINDOWPOS_UNDEFINED, width, height, 0); + if (window == NULL) { + printf("Failed to create window: %s\n", SDL_GetError()); + return; + } + + SDL_Renderer *renderer = SDL_CreateRenderer(window, -1, + SDL_RENDERER_ACCELERATED); + if (renderer == NULL) { + printf("Failed to create renderer: %s\n", SDL_GetError()); + SDL_DestroyWindow(window); + return; + } + SDL_RenderSetLogicalSize(renderer, ctx.eye->getWidth(), ctx.eye->getHeight()); + + SDL_Texture *video_tex = SDL_CreateTexture( + renderer, SDL_PIXELFORMAT_BGR24, SDL_TEXTUREACCESS_STREAMING, + ctx.eye->getWidth(), ctx.eye->getHeight()); + + if (video_tex == NULL) + { + printf("Failed to create video texture: %s\n", SDL_GetError()); + SDL_DestroyRenderer(renderer); + SDL_DestroyWindow(window); + return; + } + + (void)ctx.eye->start(); + + printf("Camera mode: %dx%d@%d\n", ctx.eye->getWidth(), ctx.eye->getHeight(), ctx.eye->getFrameRate()); + + SDL_Event e; + + Uint32 start_ticks = SDL_GetTicks(); + while (ctx.running) { + if (duration != 0 && (SDL_GetTicks() - start_ticks) / 1000 >= duration) + break; + + while (SDL_PollEvent(&e)) { + if (e.type == SDL_QUIT || (e.type == SDL_KEYUP && e.key.keysym.scancode == SDL_SCANCODE_ESCAPE)) { + ctx.running = false; + } + } + + { + Uint32 now_ticks = SDL_GetTicks(); + + ctx.last_frames++; + + if (now_ticks - ctx.last_ticks > 1000) + { + printf("FPS: %.2f\n", 1000 * ctx.last_frames / (float(now_ticks - ctx.last_ticks))); + ctx.last_ticks = now_ticks; + ctx.last_frames = 0; + } + } + + void *video_tex_pixels; + int pitch; + SDL_LockTexture(video_tex, NULL, &video_tex_pixels, &pitch); + + if (!ctx.eye->get_frame((uint8_t*) video_tex_pixels)) + ctx.running = false; + + SDL_UnlockTexture(video_tex); + + SDL_RenderCopy(renderer, video_tex, NULL, NULL); + SDL_RenderPresent(renderer); + } + + ctx.eye->stop(); + + SDL_DestroyTexture(video_tex); + SDL_DestroyRenderer(renderer); + SDL_DestroyWindow(window); +} + +int main(int argc, char *argv[]) +{ + bool mode_test = false; + int width = 640; + int height = 480; + int fps = 60; + if (argc > 1) + { + bool good_arg = false; + for (int arg_ix = 1; arg_ix < argc; ++arg_ix) + { + if (std::string(argv[arg_ix]) == "--qvga") + { + width = 320; + height = 240; + good_arg = true; + } + + if ((std::string(argv[arg_ix]) == "--fps") && argc > arg_ix) + { + std::istringstream new_fps_ss( argv[arg_ix+1] ); + if (new_fps_ss >> fps) + { + good_arg = true; + } + } + + if (std::string(argv[arg_ix]) == "--mode_test") + { + mode_test = true; + good_arg = true; + } + } + if (!good_arg) + { + std::cerr << "Usage: " << argv[0] << " [--fps XX] [--qvga] [--mode_test]" << std::endl; + } + } + + if (SDL_Init(SDL_INIT_VIDEO) < 0) { + printf("Failed to initialize SDL: %s\n", SDL_GetError()); + return EXIT_FAILURE; + } + + if (mode_test) + { + int rates_qvga[] = { 2, 3, 5, 7, 10, 12, 15, 17, 30, 37, 40, 50, 60, 75, 90, 100, 125, 137, 150, 187 }; + int num_rates_qvga = sizeof(rates_qvga) / sizeof(int); + + int rates_vga[] = { 2, 3, 5, 8, 10, 15, 20, 25, 30, 40, 50, 60, 75 }; + int num_rates_vga = sizeof(rates_vga) / sizeof(int); + + for (int index = 0; index < num_rates_qvga; ++index) + run_camera(320, 240, rates_qvga[index], 5); + + for (int index = 0; index < num_rates_vga; ++index) + run_camera(640, 480, rates_vga[index], 5); + } + else + { + run_camera(width, height, fps, 0); + } + + return EXIT_SUCCESS; +} + diff --git a/video-ps3eye/PS3EYEDriver/singleton.cpp b/video-ps3eye/PS3EYEDriver/singleton.cpp new file mode 100644 index 00000000..93017fc5 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/singleton.cpp @@ -0,0 +1,107 @@ +#include "singleton.hpp" +#include "internal.hpp" +#include "ps3eye.hpp" + +//#define USE_CONTEXT + +USBMgr::USBMgr() +{ +#ifdef USE_CONTEXT + libusb_init(&usb_context); +#else + libusb_init(nullptr); +#endif + libusb_set_debug(usb_context, 3); +} + +USBMgr::~USBMgr() +{ +#ifdef USE_CONTEXT + stop_xfer_thread(); + libusb_exit(usb_context); +#endif +} + +USBMgr& USBMgr::instance() +{ + static USBMgr ret; + return ret; +} + +void USBMgr::camera_started() +{ + if (++active_camera_count == 1) + start_xfer_thread(); +} + +void USBMgr::camera_stopped() +{ + if (--active_camera_count == 0) + stop_xfer_thread(); +} + +void USBMgr::start_xfer_thread() +{ + update_thread = std::thread(&USBMgr::transfer_loop, this); +} + +void USBMgr::stop_xfer_thread() +{ + exit_signaled = true; + update_thread.join(); + // Reset the exit signal flag. + // If we don't and we call start_xfer_thread() again, transfer_loop will exit immediately. + exit_signaled = false; +} + +void USBMgr::transfer_loop() +{ + struct timeval tv { + 0, + 50 * 1000, // ms + }; + + debug2("-> xfer loop"); + + while (!exit_signaled.load(std::memory_order_relaxed)) + { + int status = libusb_handle_events_timeout_completed(usb_context, &tv, nullptr); + + if (status != LIBUSB_SUCCESS && status != LIBUSB_ERROR_INTERRUPTED) + debug("libusb error(%d) %s\n", status, libusb_strerror((enum libusb_error)status)); + } + debug2("<- xfer loop\n"); +} + +int USBMgr::list_devices(std::vector<ps3eye_camera::device>& list) +{ + libusb_device **devs; + libusb_device_handle *devhandle; + int cnt = (int)libusb_get_device_list(usb_context, &devs); + + if (cnt < 0) + debug("device scan failed\n"); + + cnt = 0; + for (int i = 0; devs[i]; i++) + { + libusb_device* dev = devs[i]; + struct libusb_device_descriptor desc; + + if (libusb_get_device_descriptor(dev, &desc)) + continue; + if (desc.idVendor == ps3eye_camera::VENDOR_ID && desc.idProduct == ps3eye_camera::PRODUCT_ID) + { + if (libusb_open(dev, &devhandle)) + continue; + libusb_ref_device(dev); + libusb_close(devhandle); + list.emplace_back(std::make_shared<ps3eye_camera>(dev)); + cnt++; + } + } + + libusb_free_device_list(devs, 1); + + return cnt; +} diff --git a/video-ps3eye/PS3EYEDriver/singleton.hpp b/video-ps3eye/PS3EYEDriver/singleton.hpp new file mode 100644 index 00000000..7e8dc23c --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/singleton.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include <vector> +#include <memory> +#include <thread> +#include <atomic> + +struct libusb_context; +struct ps3eye_camera; + +struct USBMgr +{ + USBMgr(); + ~USBMgr(); + USBMgr(const USBMgr&) = delete; + void operator=(const USBMgr&) = delete; + + static USBMgr& instance(); + int list_devices(std::vector<std::shared_ptr<ps3eye_camera>>& list); + void camera_started(); + void camera_stopped(); + +private: + libusb_context* usb_context = nullptr; + std::thread update_thread; + std::atomic_int active_camera_count = 0; + std::atomic_bool exit_signaled = false; + + void start_xfer_thread(); + void stop_xfer_thread(); + void transfer_loop(); +}; diff --git a/video-ps3eye/PS3EYEDriver/thread-name.cpp b/video-ps3eye/PS3EYEDriver/thread-name.cpp new file mode 100644 index 00000000..e7e96b21 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/thread-name.cpp @@ -0,0 +1 @@ +#include "compat/thread-name.cpp" diff --git a/video-ps3eye/PS3EYEDriver/urb.cpp b/video-ps3eye/PS3EYEDriver/urb.cpp new file mode 100644 index 00000000..7e931490 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/urb.cpp @@ -0,0 +1,307 @@ +#include "urb.hpp" +#include "singleton.hpp" +#include "frame-queue.hpp" +#include "internal.hpp" + +#include <algorithm> +#include <cstring> + +/* Values for bmHeaderInfo (Video and Still Image Payload Headers, 2.4.3.3) */ +enum { + UVC_STREAM_EOH = (1 << 7), + UVC_STREAM_ERR = (1 << 6), + UVC_STREAM_STI = (1 << 5), + UVC_STREAM_RES = (1 << 4), + UVC_STREAM_SCR = (1 << 3), + UVC_STREAM_PTS = (1 << 2), + UVC_STREAM_EOF = (1 << 1), + UVC_STREAM_FID = (1 << 0), +}; + +void USB_CALLBACK URBDesc::transfer_completed_callback(struct libusb_transfer *xfr) +{ + URBDesc *urb = reinterpret_cast<URBDesc*>(xfr->user_data); + enum libusb_transfer_status status = xfr->status; + + switch (status) + { + case LIBUSB_TRANSFER_TIMED_OUT: + urb->frame_add(URBDesc::DISCARD_PACKET, nullptr, 0); + break; + case LIBUSB_TRANSFER_COMPLETED: + urb->pkt_scan(xfr->buffer, xfr->actual_length); + if (int error = libusb_submit_transfer(xfr); error < 0) { + debug("libusb_submit_transfer(%d), exiting\n", error); + urb->close_transfers(); + } + break; + default: + debug("transfer 0x%p failed(%d)\n", (void*)xfr, status); + urb->close_transfers(); + break; + case LIBUSB_TRANSFER_CANCELLED: + urb->transfer_cancelled(); + debug("transfer 0x%p cancelled\n", (void*)xfr); + break; + } + + //debug("length:%u, actual_length:%u\n", xfr->length, xfr->actual_length); +} + +URBDesc::~URBDesc() +{ + close_transfers(); + free_transfers(); +} + +bool URBDesc::start_transfers(libusb_device_handle* handle, uint32_t curr_frame_size) +{ + // Initialize the frame queue + frame_size = curr_frame_size; + frame_queue = std::make_shared<FrameQueue>(frame_size); + + // Initialize the current frame pointer to the start of the buffer; it will be updated as frames are completed and pushed onto the frame queue + cur_frame_start = frame_queue->ptr(); + cur_frame_data_len = 0; + + // Find the bulk transfer endpoint + uint8_t bulk_endpoint = find_ep(libusb_get_device(handle)); + libusb_clear_halt(handle, bulk_endpoint); + + // Allocate the transfer buffer + memset(transfer_buffer, 0, sizeof(transfer_buffer)); + + bool res = true; + for (int index = 0; index < NUM_TRANSFERS; ++index) + { + // Create & submit the transfer + xfers[index] = libusb_alloc_transfer(0); + + if (!xfers[index]) + { + debug("libusb_alloc_transfer failed\n"); + res = false; + break; + } + + libusb_fill_bulk_transfer(xfers[index], handle, bulk_endpoint, transfer_buffer + index * TRANSFER_SIZE, TRANSFER_SIZE, transfer_completed_callback, reinterpret_cast<void*>(this), 0); + + if (int status = libusb_submit_transfer(xfers[index]); status != LIBUSB_SUCCESS) + { + debug("libusb_submit_transfer status %d\n", status); + libusb_free_transfer(xfers[index]); + xfers[index] = nullptr; + res = false; + break; + } + + num_active_transfers++; + } + + last_pts = 0; + last_fid = 0; + + USBMgr::instance().camera_started(); + + return res; +} + +void URBDesc::close_transfers() +{ + std::unique_lock<std::mutex> lock(num_active_transfers_mutex); + + if (teardown) + return; + teardown = true; + + // Cancel any pending transfers + for (int i = 0; i < NUM_TRANSFERS; ++i) + { + if (!xfers[i]) + continue; + enum libusb_error status = (enum libusb_error)libusb_cancel_transfer(xfers[i]); + if (status) + debug("libusb_cancel_transfer error(%d) %s\n", status, libusb_strerror(status)); + } + num_active_transfers_condition.notify_one(); +} + +void URBDesc::free_transfers() +{ + std::unique_lock<std::mutex> lock(num_active_transfers_mutex); + + // Wait for cancelation to finish + num_active_transfers_condition.wait(lock, [this] { return num_active_transfers == 0; }); + + // Free completed transfers + for (libusb_transfer*& xfer : xfers) + { + if (!xfer) + continue; + libusb_free_transfer(xfer); + xfer = nullptr; + } + + USBMgr::instance().camera_stopped(); +} + +int URBDesc::transfer_cancelled() +{ + std::lock_guard<std::mutex> lock(num_active_transfers_mutex); + int refcnt = --num_active_transfers; + num_active_transfers_condition.notify_one(); + + return refcnt; +} + +void URBDesc::frame_add(enum gspca_packet_type packet_type, const uint8_t* data, int len) +{ + if (packet_type == FIRST_PACKET) + cur_frame_data_len = 0; + else + switch(last_packet_type) // ignore warning. + { + case DISCARD_PACKET: + if (packet_type == LAST_PACKET) { + last_packet_type = packet_type; + cur_frame_data_len = 0; + } + return; + case LAST_PACKET: + return; + default: + break; + } + + /* append the packet to the frame buffer */ + if (len > 0) + { + if(cur_frame_data_len + len > frame_size) + { + packet_type = DISCARD_PACKET; + cur_frame_data_len = 0; + } else { + memcpy(cur_frame_start+cur_frame_data_len, data, (size_t) len); + cur_frame_data_len += len; + } + } + + last_packet_type = packet_type; + + if (packet_type == LAST_PACKET) { + cur_frame_data_len = 0; + cur_frame_start = frame_queue->Enqueue(); + //debug("frame completed %d\n", frame_complete_ind); + } +} + +void URBDesc::pkt_scan(uint8_t* data, int len) +{ + uint32_t this_pts; + uint16_t this_fid; + int remaining_len = len; + constexpr int payload_len = 2048; // bulk type + + do { + len = std::min(remaining_len, payload_len); + + /* Payloads are prefixed with a UVC-style header. We + consider a frame to start when the FID toggles, or the PTS + changes. A frame ends when EOF is set, and we've received + the correct number of bytes. */ + + /* Verify UVC header. Header length is always 12 */ + if (data[0] != 12 || len < 12) { + debug("bad header\n"); + goto discard; + } + + /* Check errors */ + if (data[1] & UVC_STREAM_ERR) { + debug("payload error\n"); + goto discard; + } + + /* Extract PTS and FID */ + if (!(data[1] & UVC_STREAM_PTS)) { + debug("PTS not present\n"); + goto discard; + } + + this_pts = (data[5] << 24) | (data[4] << 16) | (data[3] << 8) | data[2]; + this_fid = (uint16_t) ((data[1] & UVC_STREAM_FID) ? 1 : 0); + + /* If PTS or FID has changed, start a new frame. */ + if (this_pts != last_pts || this_fid != last_fid) { + if (last_packet_type == INTER_PACKET) + { + /* The last frame was incomplete, so don't keep it or we will glitch */ + frame_add(DISCARD_PACKET, nullptr, 0); + } + last_pts = this_pts; + last_fid = this_fid; + frame_add(FIRST_PACKET, data + 12, len - 12); + } /* If this packet is marked as EOF, end the frame */ + else if (data[1] & UVC_STREAM_EOF) + { + last_pts = 0; + if (cur_frame_data_len + len - 12 != frame_size) + goto discard; + frame_add(LAST_PACKET, data + 12, len - 12); + } else { + /* Add the data from this payload */ + frame_add(INTER_PACKET, data + 12, len - 12); + } + + /* Done this payload */ + goto scan_next; + +discard: + /* Discard data until a new frame starts. */ + frame_add(DISCARD_PACKET, nullptr, 0); +scan_next: + remaining_len -= len; + data += len; + } while (remaining_len > 0); +} + +/* + * look for an input transfer endpoint in an alternate setting + * libusb_endpoint_descriptor + */ +uint8_t URBDesc::find_ep(struct libusb_device *device) +{ + const struct libusb_interface_descriptor *altsetting = nullptr; + const struct libusb_endpoint_descriptor *ep; + struct libusb_config_descriptor *config = nullptr; + int i; + uint8_t ep_addr = 0; + + libusb_get_active_config_descriptor(device, &config); + + if (!config) return 0; + + for (i = 0; i < config->bNumInterfaces; i++) { + altsetting = config->interface[i].altsetting; + if (altsetting[0].bInterfaceNumber == 0) { + break; + } + } + + if (altsetting) + for (i = 0; i < altsetting->bNumEndpoints; i++) { + ep = &altsetting->endpoint[i]; + if ((ep->bmAttributes & LIBUSB_TRANSFER_TYPE_MASK) == LIBUSB_TRANSFER_TYPE_BULK + && ep->wMaxPacketSize != 0) + { + ep_addr = ep->bEndpointAddress; + break; + } + } + + libusb_free_config_descriptor(config); + + return ep_addr; +} + + diff --git a/video-ps3eye/PS3EYEDriver/urb.hpp b/video-ps3eye/PS3EYEDriver/urb.hpp new file mode 100644 index 00000000..3bee3919 --- /dev/null +++ b/video-ps3eye/PS3EYEDriver/urb.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include <memory> +#include <cstdint> +#include <mutex> + +struct FrameQueue; +struct libusb_device; +struct libusb_transfer; +struct libusb_device_handle; + +#ifdef _WIN32 +# define USB_CALLBACK __stdcall +#else +# define USB_CALLBACK +#endif + +struct URBDesc +{ + URBDesc() = default; + ~URBDesc(); + + bool start_transfers(libusb_device_handle *handle, uint32_t curr_frame_size); + void free_transfers(); + void close_transfers(); + + FrameQueue& queue() { return *frame_queue; } + +private: + enum { + TRANSFER_SIZE = 65536, + NUM_TRANSFERS = 5, + }; + + /* packet types when moving from iso buf to frame buf */ + enum gspca_packet_type { + DISCARD_PACKET, + FIRST_PACKET, + INTER_PACKET, + LAST_PACKET, + }; + + std::shared_ptr<FrameQueue> frame_queue; + std::mutex num_active_transfers_mutex; + std::condition_variable num_active_transfers_condition; + + gspca_packet_type last_packet_type = DISCARD_PACKET; + libusb_transfer* xfers[NUM_TRANSFERS] {}; + uint8_t* cur_frame_start = nullptr; + uint32_t cur_frame_data_len = 0; + uint32_t frame_size = 0; + uint32_t last_pts = 0; + uint16_t last_fid = 0; + uint8_t transfer_buffer[TRANSFER_SIZE * NUM_TRANSFERS]; + uint8_t num_active_transfers = 0; + bool teardown = false; + + int transfer_cancelled(); + void frame_add(enum gspca_packet_type packet_type, const uint8_t *data, int len); + void pkt_scan(uint8_t *data, int len); + static uint8_t find_ep(struct libusb_device *device); + static void USB_CALLBACK transfer_completed_callback(struct libusb_transfer *xfr); +}; |