diff --git a/src/Frame.cpp b/src/Frame.cpp index ab0d27b5f5..f5507480c3 100644 --- a/src/Frame.cpp +++ b/src/Frame.cpp @@ -11,4 +11,7 @@ using namespace cs; -void Frame::ReleaseFrame() { m_source->ReleaseFrame(m_data); } +void Frame::ReleaseFrame() { + m_source->ReleaseFrame(std::unique_ptr(m_data)); + m_data = nullptr; +} diff --git a/src/Frame.h b/src/Frame.h index 3e4d1c164c..f0ae61de14 100644 --- a/src/Frame.h +++ b/src/Frame.h @@ -10,10 +10,11 @@ #include #include +#include -#include "llvm/SmallVector.h" +#include "llvm/StringRef.h" -#include "Image.h" +#include "cameraserver_cpp.h" namespace cs { @@ -22,17 +23,29 @@ class SourceImpl; class Frame { friend class SourceImpl; + public: + typedef std::chrono::system_clock::time_point Time; + + private: struct Data { + explicit Data(std::size_t capacity_) + : data(new char[capacity_]), size(0), capacity(capacity_) {} + ~Data() { delete[] data; } + std::atomic_int refcount{0}; - std::chrono::system_clock::time_point timestamp; - Image image; + Time time; + char* data; + std::size_t size; + std::size_t capacity; + VideoMode::PixelFormat pixelFormat; }; public: Frame() noexcept : m_source{nullptr}, m_data{nullptr} {} - Frame(SourceImpl& source, Data* data) noexcept : m_source{&source}, - m_data{data} { + Frame(SourceImpl& source, std::unique_ptr data) noexcept + : m_source{&source}, + m_data{data.release()} { if (m_data) ++(m_data->refcount); } @@ -52,19 +65,29 @@ class Frame { explicit operator bool() const { return m_data; } + operator llvm::StringRef() const { + if (!m_data) return llvm::StringRef{}; + return llvm::StringRef(m_data->data, m_data->size); + } + std::size_t size() const { if (!m_data) return 0; - return m_data->image.size(); + return m_data->size; } const char* data() const { if (!m_data) return nullptr; - return m_data->image.data(); + return m_data->data; } - std::chrono::system_clock::time_point time() const { - if (!m_data) return std::chrono::system_clock::time_point{}; - return m_data->timestamp; + VideoMode::PixelFormat GetPixelFormat() const { + if (!m_data) return VideoMode::kUnknown; + return m_data->pixelFormat; + } + + Time time() const { + if (!m_data) return Time{}; + return m_data->time; } friend void swap(Frame& first, Frame& second) noexcept { diff --git a/src/HTTPSinkImpl.cpp b/src/HTTPSinkImpl.cpp index 125db57cf9..a694ea494e 100644 --- a/src/HTTPSinkImpl.cpp +++ b/src/HTTPSinkImpl.cpp @@ -134,6 +134,8 @@ bool HTTPSinkImpl::UnescapeURI(llvm::StringRef str, // Perform a command specified by HTTP GET parameters. bool HTTPSinkImpl::ProcessCommand(llvm::raw_ostream& os, SourceImpl& source, llvm::StringRef parameters, bool respond) { + llvm::SmallString<256> responseBuf; + llvm::raw_svector_ostream response{responseBuf}; // command format: param1=value1¶m2=value2... while (!parameters.empty()) { // split out next param and value @@ -142,6 +144,8 @@ bool HTTPSinkImpl::ProcessCommand(llvm::raw_ostream& os, SourceImpl& source, if (rawParam.empty()) continue; // ignore "&&" std::tie(rawParam, rawValue) = rawParam.split('='); if (rawParam.empty() || rawValue.empty()) continue; // ignore "param=" + DEBUG4("HTTP parameter \"" << rawParam << "\" value \"" << rawValue + << "\""); // unescape param llvm::SmallString<64> param; @@ -168,6 +172,7 @@ bool HTTPSinkImpl::ProcessCommand(llvm::raw_ostream& os, SourceImpl& source, // try to assign parameter auto prop = source.GetPropertyIndex(param); if (!prop) { + response << param << ": \"ignored\"\r\n"; WARNING("ignoring HTTP parameter \"" << param << "\""); continue; } @@ -180,13 +185,19 @@ bool HTTPSinkImpl::ProcessCommand(llvm::raw_ostream& os, SourceImpl& source, case CS_PROP_ENUM: { int val; if (value.str().getAsInteger(10, val)) { + response << param << ": \"invalid integer\"\r\n"; WARNING("HTTP parameter \"" << param << "\" value \"" << value << "\" is not an integer"); - } else + } else { + response << param << ": " << val << "\r\n"; + DEBUG4("HTTP parameter \"" << param << "\" value " << value); source.SetProperty(prop, val, &status); + } break; } case CS_PROP_STRING: { + response << param << ": \"ok\"\r\n"; + DEBUG4("HTTP parameter \"" << param << "\" value \"" << value << "\""); source.SetStringProperty(prop, value, &status); break; } @@ -195,10 +206,10 @@ bool HTTPSinkImpl::ProcessCommand(llvm::raw_ostream& os, SourceImpl& source, } } + // Send HTTP response if (respond) { - // Send HTTP response SendHeader(os, 200, "OK", "text/plain"); - //os << command << ": " << res; + os << response.str() << "\r\n"; } return true; @@ -212,13 +223,13 @@ void HTTPSinkImpl::SendJSON(llvm::raw_ostream& os, SourceImpl& source, os << "{\n\"controls\": [\n"; llvm::SmallVector properties_vec; bool first = true; - for (auto prop : source.EnumerateProperties(properties_vec)) { + CS_Status status = 0; + for (auto prop : source.EnumerateProperties(properties_vec, &status)) { if (first) first = false; else os << ",\n"; os << "{"; - CS_Status status = 0; llvm::SmallString<128> name_buf; auto name = source.GetPropertyName(prop, name_buf, &status); auto type = source.GetPropertyType(prop); @@ -319,7 +330,7 @@ void HTTPSinkImpl::SendStream(wpi::raw_socket_ostream& os) { oss << "--" BOUNDARY "\r\n"; os << oss.str(); - DEBUG("Headers send, sending stream now"); + DEBUG("HTTP: Headers send, sending stream now"); Enable(); while (m_active && !os.has_error()) { @@ -329,6 +340,7 @@ void HTTPSinkImpl::SendStream(wpi::raw_socket_ostream& os) { std::this_thread::sleep_for(std::chrono::seconds(1)); continue; } + DEBUG4("HTTP: waiting for frame"); Frame frame = source->GetNextFrame(); // blocks if (!m_active) break; if (!frame) { @@ -336,6 +348,7 @@ void HTTPSinkImpl::SendStream(wpi::raw_socket_ostream& os) { std::this_thread::sleep_for(std::chrono::milliseconds(10)); continue; } + DEBUG4("HTTP: sending frame size=" << frame.size()); // print the individual mimetype and the length // sending the content-length fixes random stream disruption observed @@ -349,7 +362,7 @@ void HTTPSinkImpl::SendStream(wpi::raw_socket_ostream& os) { << "X-Timestamp: " << timestamp << "\r\n" << "\r\n"; os << oss.str(); - os << frame.data(); + os << llvm::StringRef(frame.data(), frame.size()); os << "\r\n--" BOUNDARY "\r\n"; // os.flush(); } @@ -359,12 +372,15 @@ void HTTPSinkImpl::SendStream(wpi::raw_socket_ostream& os) { // thread for clients that connected to this server void HTTPSinkImpl::ConnThreadMain(wpi::NetworkStream* stream) { wpi::raw_socket_istream is{*stream}; + wpi::raw_socket_ostream os{*stream, true}; // Read the request string from the stream llvm::SmallString<128> buf; - if (!ReadLine(is, buf, 4096)) return; + if (!ReadLine(is, buf, 4096)) { + DEBUG("HTTP error getting request string"); + return; + } - wpi::raw_socket_ostream os{*stream, true}; enum { kCommand, kStream, kGetSettings } type; llvm::StringRef parameters; size_t pos; @@ -402,9 +418,10 @@ void HTTPSinkImpl::ConnThreadMain(wpi::NetworkStream* stream) { // Read the rest of the HTTP request. // The end of the request is marked by a single, empty line with "\r\n" + llvm::SmallString<128> buf2; do { - if (!ReadLine(is, buf, 4096)) return; - } while (!buf.startswith("\r\n")); + if (!ReadLine(is, buf2, 4096)) return; + } while (!buf2.startswith("\r\n")); // Send response switch (type) { @@ -417,8 +434,7 @@ void HTTPSinkImpl::ConnThreadMain(wpi::NetworkStream* stream) { break; case kCommand: if (auto source = GetSource()) { - if (!ProcessCommand(os, *source, parameters, true)) return; - SendHeader(os, 200, "OK", "text/plain"); + ProcessCommand(os, *source, parameters, true); } else { SendHeader(os, 200, "OK", "text/plain"); os << "Ignored due to no connected source." << "\r\n"; diff --git a/src/Image.h b/src/Image.h deleted file mode 100644 index d0b3a9ed8c..0000000000 --- a/src/Image.h +++ /dev/null @@ -1,55 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) FIRST 2016. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -#ifndef CAMERASERVER_IMAGE_H_ -#define CAMERASERVER_IMAGE_H_ - -#include - -namespace cs { - -class Image { - public: - enum Type { - kUnknown = 0, - kJpeg - }; - - Image() : m_data(nullptr), m_size(0), m_capacity(0) {} - explicit Image(std::size_t capacity) - : m_data(new char[capacity]), m_capacity(capacity) {} - Image(Image&& image) - : m_data(image.m_data), - m_size(image.m_size), - m_capacity(image.m_capacity) { - image.m_data = nullptr; - image.m_size = 0; - image.m_capacity = 0; - } - ~Image() { delete[] m_data; } - Image(const Image&) = delete; - Image& operator=(const Image&) = delete; - - char* data() { return m_data; } - const char* data() const { return m_data; } - std::size_t size() const { return m_size; } - std::size_t capacity() const { return m_capacity; } - Type type() const { return m_type; } - - void SetSize(std::size_t size) { m_size = size; } - void SetType(Type type) { m_type = type; } - - private: - char* m_data; - std::size_t m_size; - std::size_t m_capacity; - Type m_type = kUnknown; -}; - -} // namespace cs - -#endif // CAMERASERVER_IMAGE_H_ diff --git a/src/SourceImpl.cpp b/src/SourceImpl.cpp index 7138d90986..ffea5e0b06 100644 --- a/src/SourceImpl.cpp +++ b/src/SourceImpl.cpp @@ -7,18 +7,28 @@ #include "SourceImpl.h" -#include "llvm/STLExtras.h" +#include +#include + +#include "Log.h" using namespace cs; +static constexpr std::size_t kMaxFramesAvail = 32; + SourceImpl::SourceImpl(llvm::StringRef name) : m_name{name}, m_frame{*this, nullptr} {} SourceImpl::~SourceImpl() { // Wake up anyone who is waiting. This also clears the current frame, // which is good because its destructor will call back into the class. - EnableSink(); Wakeup(); + // Set a flag so ReleaseFrame() doesn't re-add them to m_framesAvail. + // Put in a block so we destroy before the destructor ends. + { + m_destroyFrames = true; + auto frames = std::move(m_framesAvail); + } // Everything else can clean up itself. } @@ -60,33 +70,72 @@ bool SourceImpl::SetFPS(int fps, CS_Status* status) { return SetVideoMode(mode, status); } -void SourceImpl::StartFrame() { - std::lock_guard lock{m_mutex}; - if (m_frameData) return; - if (m_framesAvail.empty()) { - m_frameData = llvm::make_unique(); - } else { - m_frameData = std::move(m_framesAvail.back()); - m_framesAvail.pop_back(); - m_frameData->refcount = 0; - } -} - -//TODO: Image& SourceImpl::AddImage(std::size_t size) {} - -void SourceImpl::FinishFrame() { +void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, + llvm::StringRef data, Frame::Time time) { + std::unique_ptr frameData; { std::lock_guard lock{m_mutex}; - std::lock_guard lock2{m_frameMutex}; - m_frame = Frame{*this, m_frameData.release()}; + // find the smallest existing frame that is at least big enough. + int found = -1; + for (std::size_t i = 0; i < m_framesAvail.size(); ++i) { + // is it big enough? + if (m_framesAvail[i] && m_framesAvail[i]->capacity >= data.size()) { + // is it smaller than the last found? + if (found < 0 || + m_framesAvail[i]->capacity < m_framesAvail[found]->capacity) { + // yes, update + found = i; + } + } + } + + // if nothing found, allocate a new buffer + if (found < 0) + frameData.reset(new Frame::Data{data.size()}); + else + frameData = std::move(m_framesAvail[found]); } + + // Initialize frame data + frameData->refcount = 0; + frameData->time = time; + frameData->size = data.size(); + frameData->pixelFormat = pixelFormat; + + // Copy in image data + DEBUG4("Copying data to " << ((void*)frameData->data) << " from " + << ((void*)data.data()) << " (" << data.size() + << " bytes)"); + std::memcpy(frameData->data, data.data(), data.size()); + + // Update frame + { + std::lock_guard lock{m_frameMutex}; + m_frame = Frame{*this, std::move(frameData)}; + } + + // Signal listeners m_frameCv.notify_all(); } -void SourceImpl::ReleaseFrame(Frame::Data* data) { +void SourceImpl::ReleaseFrame(std::unique_ptr data) { std::lock_guard lock{m_mutex}; - // Return the image to the pool - m_imagesAvail.emplace_back(std::move(data->image)); - // Return the frame to the pool - m_framesAvail.emplace_back(data); + if (m_destroyFrames) return; + // Return the frame to the pool. First try to find an empty slot, otherwise + // add it to the end. + auto it = std::find(m_framesAvail.begin(), m_framesAvail.end(), nullptr); + if (it != m_framesAvail.end()) + (*it) = std::move(data); + else if (m_framesAvail.size() > kMaxFramesAvail) { + // Replace smallest buffer; don't need to check for null because the above + // find would have found it. + auto it2 = std::min_element(m_framesAvail.begin(), m_framesAvail.end(), + [](const std::unique_ptr& a, + const std::unique_ptr& b) { + return a->capacity < b->capacity; + }); + if ((*it2)->capacity < data->capacity) + *it2 = std::move(data); + } else + m_framesAvail.emplace_back(std::move(data)); } diff --git a/src/SourceImpl.h b/src/SourceImpl.h index 56591a5475..8f5e4c4d4f 100644 --- a/src/SourceImpl.h +++ b/src/SourceImpl.h @@ -34,38 +34,35 @@ class SourceImpl { llvm::StringRef GetName() const { return m_name; } virtual llvm::StringRef GetDescription( llvm::SmallVectorImpl& buf) const = 0; - bool IsConnected() const { return m_connected; } + virtual bool IsConnected() const = 0; // Functions to keep track of the overall number of sinks connected to this // source. Primarily used by sinks to determine if other sinks are using // the same source. int GetNumSinks() const { return m_numSinks; } - void AddSink() { ++m_numSinks; } - void RemoveSink() { --m_numSinks; } + void AddSink() { + ++m_numSinks; + NumSinksChanged(); + } + void RemoveSink() { + --m_numSinks; + NumSinksChanged(); + } // Functions to keep track of the number of sinks connected to this source // that are "enabled", in other words, listening for new images. Primarily // used by sources to determine whether they should actually bother trying // to get source frames. - int GetNumSinksEnabled() const { - std::lock_guard lock{m_numSinksEnabledMutex}; - return m_numSinksEnabled; - } + int GetNumSinksEnabled() const { return m_numSinksEnabled; } void EnableSink() { - std::lock_guard lock{m_numSinksEnabledMutex}; ++m_numSinksEnabled; - m_numSinksEnabledCv.notify_all(); + NumSinksEnabledChanged(); } void DisableSink() { - std::lock_guard lock{m_numSinksEnabledMutex}; --m_numSinksEnabled; - } - - void WaitForEnabledSink() { - std::unique_lock lock{m_numSinksEnabledMutex}; - m_numSinksEnabledCv.wait(lock, [this] { return m_numSinksEnabled > 0; }); + NumSinksEnabledChanged(); } // Gets the current frame (without waiting for a new one). @@ -80,7 +77,7 @@ class SourceImpl { // Property functions virtual int GetPropertyIndex(llvm::StringRef name) const = 0; virtual llvm::ArrayRef EnumerateProperties( - llvm::SmallVectorImpl& vec) const = 0; + llvm::SmallVectorImpl& vec, CS_Status* status) const = 0; virtual CS_PropertyType GetPropertyType(int property) const = 0; virtual llvm::StringRef GetPropertyName(int property, llvm::SmallVectorImpl& buf, @@ -102,8 +99,6 @@ class SourceImpl { // Video mode functions virtual VideoMode GetVideoMode(CS_Status* status) const = 0; virtual bool SetVideoMode(const VideoMode& mode, CS_Status* status) = 0; - virtual std::vector EnumerateVideoModes( - CS_Status* status) const = 0; // These have default implementations but can be overridden for custom // or optimized behavior. @@ -112,38 +107,38 @@ class SourceImpl { virtual bool SetResolution(int width, int height, CS_Status* status); virtual bool SetFPS(int fps, CS_Status* status); - protected: - void StartFrame(); - Image& AddImage(std::size_t size); - void FinishFrame(); + virtual std::vector EnumerateVideoModes( + CS_Status* status) const = 0; + + protected: + void PutFrame(VideoMode::PixelFormat pixelFormat, llvm::StringRef data, + Frame::Time time); + + // Notification functions for corresponding atomics + virtual void NumSinksChanged() = 0; + virtual void NumSinksEnabledChanged() = 0; - std::atomic_bool m_connected{false}; std::atomic_int m_numSinks{0}; + std::atomic_int m_numSinksEnabled{0}; private: - void ReleaseFrame(Frame::Data* data); + void ReleaseFrame(std::unique_ptr data); std::string m_name; std::mutex m_mutex; - mutable std::mutex m_numSinksEnabledMutex; - std::condition_variable m_numSinksEnabledCv; - int m_numSinksEnabled; - std::mutex m_frameMutex; std::condition_variable m_frameCv; - // Most recent complete frame (returned to callers of GetNextFrame) + // Most recent frame (returned to callers of GetNextFrame) // Access protected by m_frameMutex. Frame m_frame; - // In-progress frame. Will be moved to m_frame when complete. - std::unique_ptr m_frameData; + bool m_destroyFrames{false}; - // Pools of frames and images to reduce malloc traffic. + // Pool of frame data to reduce malloc traffic. std::vector> m_framesAvail; - std::vector m_imagesAvail; }; } // namespace cs diff --git a/src/USBCameraBuffer.h b/src/USBCameraBuffer.h new file mode 100644 index 0000000000..330763e207 --- /dev/null +++ b/src/USBCameraBuffer.h @@ -0,0 +1,58 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) FIRST 2016. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +#ifndef CAMERASERVER_USBCAMERABUFFER_H_ +#define CAMERASERVER_USBCAMERABUFFER_H_ + +#ifdef __linux__ +#include +#endif + +namespace cs { + +class USBCameraBuffer { + public: + USBCameraBuffer() noexcept : m_data{nullptr}, m_length{0} {} + USBCameraBuffer(USBCameraBuffer&& other) noexcept : USBCameraBuffer() { + swap(*this, other); + } + USBCameraBuffer& operator=(USBCameraBuffer&& other) noexcept { + swap(*this, other); + return *this; + } + USBCameraBuffer(const USBCameraBuffer&) = delete; + USBCameraBuffer& operator=(const USBCameraBuffer&) = delete; + +#ifdef __linux__ + USBCameraBuffer(int fd, size_t length, off_t offset) noexcept + : m_length{length} { + m_data = + mmap(nullptr, length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, offset); + if (m_data == MAP_FAILED) { + m_data = nullptr; + m_length = 0; + } + } + + ~USBCameraBuffer() { + if (m_data) munmap(m_data, m_length); + } +#endif + + friend void swap(USBCameraBuffer& first, USBCameraBuffer& second) noexcept { + using std::swap; + swap(first.m_data, second.m_data); + swap(first.m_length, second.m_length); + } + + void* m_data; + size_t m_length; +}; + +} // namespace cs + +#endif // CAMERASERVER_USBCAMERABUFFER_H_ diff --git a/src/USBCameraImpl.cpp b/src/USBCameraImpl.cpp index f75b3c9998..a2e08836f2 100644 --- a/src/USBCameraImpl.cpp +++ b/src/USBCameraImpl.cpp @@ -12,11 +12,16 @@ #ifdef __linux__ #include #include +#include #include #include #include +#include +#include #include +#include #include +#include #include #include @@ -80,10 +85,6 @@ static llvm::StringRef NormalizeName(llvm::StringRef name, return llvm::StringRef(buf.data(), buf.size()); } -static inline int ControlIdToProperty(__u32 control_id) { - return (control_id & 0xffff) + 1; -} - #ifdef VIDIOC_QUERY_EXT_CTRL USBCameraImpl::PropertyData::PropertyData( const struct v4l2_query_ext_ctrl& ctrl) @@ -157,18 +158,26 @@ USBCameraImpl::PropertyData::PropertyData(const struct v4l2_queryctrl& ctrl) } static inline int CheckedIoctl(int fd, unsigned long req, void* data, - const char* name, bool quiet) { + const char* name, const char* file, int line, + bool quiet) { int retval = ioctl(fd, req, data); - if (!quiet && retval < 0) - ERROR("ioctl " << name << "failed: " << std::strerror(errno)); + if (!quiet && retval < 0) { + llvm::SmallString<64> localfile{file}; + localfile.push_back('\0'); + ERROR("ioctl " << name << " failed at " << basename(localfile.data()) << ":" + << line << ": " << std::strerror(errno)); + } return retval; } -#define DoIoctl(fd, req, data) CheckedIoctl(fd, req, data, #req, false) -#define TryIoctl(fd, req, data) CheckedIoctl(fd, req, data, #req, true) +#define DoIoctl(fd, req, data) \ + CheckedIoctl(fd, req, data, #req, __FILE__, __LINE__, false) +#define TryIoctl(fd, req, data) \ + CheckedIoctl(fd, req, data, #req, __FILE__, __LINE__, true) static int ExtCtrlIoctl(int fd, __u32* id, USBCameraImpl::PropertyData* prop) { int rc; + bool done = false; #ifdef VIDIOC_QUERY_EXT_CTRL v4l2_query_ext_ctrl qc_ext; std::memset(&qc_ext, 0, sizeof(qc_ext)); @@ -179,17 +188,31 @@ static int ExtCtrlIoctl(int fd, __u32* id, USBCameraImpl::PropertyData* prop) { // We don't support array types if (qc_ext.elems > 1 || qc_ext.nr_of_dims > 0) return 0; *prop = qc_ext; - return 0; + done = true; } #endif - // Fall back to normal QUERYCTRL - struct v4l2_queryctrl qc; - std::memset(&qc, 0, sizeof(qc)); - qc.id = *id; - rc = TryIoctl(fd, VIDIOC_QUERYCTRL, &qc); - *id = qc.id; // copy back - if (rc != 0) return rc; - *prop = qc; + if (!done) { + // Fall back to normal QUERYCTRL + struct v4l2_queryctrl qc; + std::memset(&qc, 0, sizeof(qc)); + qc.id = *id; + rc = TryIoctl(fd, VIDIOC_QUERYCTRL, &qc); + *id = qc.id; // copy back + if (rc != 0) return rc; + *prop = qc; + } + + // Cache enum property choices + if (prop->propType != CS_PROP_ENUM) return 0; + prop->enumChoices.resize(prop->maximum + 1); + v4l2_querymenu qmenu; + std::memset(&qmenu, 0, sizeof(qmenu)); + qmenu.id = *id; + for (int i = prop->minimum; i <= prop->maximum; ++i) { + qmenu.index = static_cast<__u32>(i); + if (TryIoctl(fd, VIDIOC_QUERYMENU, &qmenu) != 0) continue; + prop->enumChoices[i] = reinterpret_cast(qmenu.name); + } return 0; } @@ -202,6 +225,7 @@ static int GetIntCtrlIoctl(int fd, unsigned id, int type, int64_t* value) { struct v4l2_ext_control ctrl; struct v4l2_ext_controls ctrls; std::memset(&ctrl, 0, sizeof(ctrl)); + std::memset(&ctrls, 0, sizeof(ctrls)); ctrl.id = id; ctrls.ctrl_class = ctrl_class; ctrls.count = 1; @@ -212,6 +236,7 @@ static int GetIntCtrlIoctl(int fd, unsigned id, int type, int64_t* value) { } else { // Use normal control struct v4l2_control ctrl; + std::memset(&ctrl, 0, sizeof(ctrl)); ctrl.id = id; int rc = DoIoctl(fd, VIDIOC_G_CTRL, &ctrl); if (rc < 0) return rc; @@ -229,6 +254,7 @@ static int SetIntCtrlIoctl(int fd, unsigned id, int type, int64_t value) { struct v4l2_ext_control ctrl; struct v4l2_ext_controls ctrls; std::memset(&ctrl, 0, sizeof(ctrl)); + std::memset(&ctrls, 0, sizeof(ctrls)); ctrl.id = id; if (type == V4L2_CTRL_TYPE_INTEGER64) ctrl.value64 = value; @@ -247,6 +273,43 @@ static int SetIntCtrlIoctl(int fd, unsigned id, int type, int64_t value) { } } +static int GetStringCtrlIoctl(int fd, int id, int maximum, + std::string* value) { + struct v4l2_ext_control ctrl; + struct v4l2_ext_controls ctrls; + std::memset(&ctrl, 0, sizeof(ctrl)); + std::memset(&ctrls, 0, sizeof(ctrls)); + ctrl.id = id; + ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(id); + ctrls.count = 1; + ctrls.controls = &ctrl; + int rc = DoIoctl(fd, VIDIOC_G_EXT_CTRLS, &ctrls); + if (rc < 0) { + value->clear(); + return rc; + } + value->assign(ctrl.string, std::strlen(ctrl.string)); + return 0; +} + +static int SetStringCtrlIoctl(int fd, int id, int maximum, + llvm::StringRef value) { + llvm::SmallString<64> str{value.substr( + 0, std::min(value.size(), static_cast(maximum)))}; + + struct v4l2_ext_control ctrl; + struct v4l2_ext_controls ctrls; + std::memset(&ctrl, 0, sizeof(ctrl)); + std::memset(&ctrls, 0, sizeof(ctrls)); + ctrl.id = id; + ctrl.size = str.size(); + ctrl.string = const_cast(str.c_str()); + ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(id); + ctrls.count = 1; + ctrls.controls = &ctrl; + return DoIoctl(fd, VIDIOC_S_EXT_CTRLS, &ctrls); +} + static std::string GetDescriptionImpl(const char* cpath) { llvm::StringRef path{cpath}; int fd; @@ -282,6 +345,7 @@ static std::string GetDescriptionImpl(const char* cpath) { fd = open(cpath, O_RDWR); if (fd >= 0) { struct v4l2_capability vcap; + std::memset(&vcap, 0, sizeof(vcap)); if (DoIoctl(fd, VIDIOC_QUERYCAP, &vcap) >= 0) { close(fd); llvm::StringRef card{reinterpret_cast(vcap.card)}; @@ -312,325 +376,535 @@ USBCameraImpl::USBCameraImpl(llvm::StringRef name, llvm::StringRef path) : SourceImpl{name}, m_path{path}, m_description{GetDescriptionImpl(m_path.c_str())}, - m_fd{open(m_path.c_str(), O_RDWR)}, - m_active{false} { - if (m_fd >= 0) { - m_connected = true; - struct v4l2_capability vcap; - if (DoIoctl(m_fd, VIDIOC_QUERYCAP, &vcap) >= 0) { - m_capabilities = vcap.capabilities; - if (m_capabilities & V4L2_CAP_DEVICE_CAPS) - m_capabilities = vcap.device_caps; - } - CS_Status status = 0; - m_mode = GetVideoMode(&status); + m_fd{-1}, + m_command_fd{eventfd(0, 0)}, + m_active{true} { + // Kick off the camera thread + m_cameraThread = std::thread(&USBCameraImpl::CameraThreadMain, this); +} + +USBCameraImpl::~USBCameraImpl() { + m_active = false; + + // Just in case anyone is waiting... + m_responseCv.notify_all(); + + // Send message to wake up thread; select timeout will wake us up anyway, + // but this speeds shutdown. + Send(CreateMessage(Message::kNone)); + + // join camera thread + m_cameraThread.join(); + + // close command fd + int fd = m_command_fd.exchange(-1); + if (fd >= 0) close(fd); +} + +static inline void DoFdSet(int fd, fd_set* set, int* nfds) { + if (fd >= 0) { + FD_SET(fd, set); + if ((fd + 1) > *nfds) *nfds = fd + 1; } } -USBCameraImpl::~USBCameraImpl() { Stop(); } +void USBCameraImpl::CameraThreadMain() { + // We want to be notified on file creation and deletion events in the device + // path. This is used to detect disconnects and reconnects. + std::unique_ptr notify_is; + int notify_fd = inotify_init(); + if (notify_fd >= 0) { + // need to make a copy as dirname can modify it + llvm::SmallString<64> pathCopy{m_path}; + pathCopy.push_back('\0'); + if (inotify_add_watch(notify_fd, dirname(pathCopy.data()), + IN_CREATE | IN_DELETE) < 0) { + close(notify_fd); + notify_fd = -1; + } else { + notify_is.reset(new wpi::raw_fd_istream{ + notify_fd, true, sizeof(struct inotify_event) + NAME_MAX + 1}); + } + } + bool notified = (notify_fd < 0); // treat as always notified if cannot notify -llvm::StringRef USBCameraImpl::GetDescription( - llvm::SmallVectorImpl& buf) const { - return m_description; + // Get the basename for later notify use + llvm::SmallString<64> pathCopy{m_path}; + pathCopy.push_back('\0'); + llvm::SmallString<64> base{basename(pathCopy.data())}; + + // Used to restart streaming on reconnect + bool wasStreaming = false; + + // Default to not streaming + m_streaming = false; + + + while (m_active) { + // If not connected, try to reconnect + if (m_fd < 0) DeviceConnect(); + + // Make copies of fd's in case they go away + int command_fd = m_command_fd.load(); + int fd = m_fd.load(); + if (!m_active) break; + + // Reset notified flag and restart streaming if necessary + if (fd >= 0) { + notified = (notify_fd < 0); + if (wasStreaming && !m_streaming) { + DeviceStreamOn(); + wasStreaming = false; + } + } + + // Turn off streaming if no one is listening, and turn it on if there is. + if (m_streaming && m_numSinksEnabled == 0) { + DeviceStreamOff(); + } else if (!m_streaming && m_numSinksEnabled > 0) { + DeviceStreamOn(); + } + + // The select timeout can be long unless we're trying to reconnect + struct timeval tv; + if (fd < 0 && notified) { + tv.tv_sec = 0; + tv.tv_usec = 300000; + } else { + tv.tv_sec = 2; + tv.tv_usec = 0; + } + + // select on applicable read descriptors + int nfds = 0; + fd_set readfds; + FD_ZERO(&readfds); + DoFdSet(command_fd, &readfds, &nfds); + if (m_streaming) DoFdSet(fd, &readfds, &nfds); + DoFdSet(notify_fd, &readfds, &nfds); + + if (select(nfds, &readfds, nullptr, nullptr, &tv) < 0) { + ERROR("USB select(): " << strerror(errno)); + break; // XXX: is this the right thing to do here? + } + + // Double-check to see if we're shutting down + if (!m_active) break; + + // Handle notify events + if (notify_fd >= 0 && FD_ISSET(notify_fd, &readfds)) { + DEBUG4("USB " << m_path << ": notify event"); + struct inotify_event event; + do { + // Read the event structure + notify_is->read(&event, sizeof(event)); + // Read the event name + llvm::SmallString<64> raw_name; + raw_name.resize(event.len); + notify_is->read(raw_name.data(), event.len); + // If the name is what we expect... + llvm::StringRef name{raw_name.c_str()}; + DEBUG4("USB " << m_path << ": got event on '" << name << "' (" + << name.size() << ") compare to '" << base << "' (" + << base.size() << ") mask " << event.mask); + if (name == base) { + if ((event.mask & IN_DELETE) != 0) { + wasStreaming = m_streaming; + DeviceStreamOff(); + DeviceDisconnect(); + } else if ((event.mask & IN_CREATE) != 0) { + notified = true; + } + } + } while (!notify_is->has_error() && + notify_is->in_avail() >= sizeof(event)); + continue; + } + + // Handle commands + if (command_fd >= 0 && FD_ISSET(command_fd, &readfds)) { + DEBUG4("USB " << m_path << ": got command"); + // Read it to clear + eventfd_t val; + eventfd_read(command_fd, &val); + DeviceProcessCommands(); + continue; + } + + // Handle frames + if (m_streaming && fd >= 0 && FD_ISSET(fd, &readfds)) { + DEBUG4("USB " << m_path << ": grabbing image"); + + // Dequeue buffer + struct v4l2_buffer buf; + std::memset(&buf, 0, sizeof(buf)); + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (DoIoctl(fd, VIDIOC_DQBUF, &buf) != 0) { + WARNING("USB " << m_path << ": could not dequeue buffer"); + wasStreaming = m_streaming; + DeviceStreamOff(); + DeviceDisconnect(); + notified = true; // device wasn't deleted, just error'ed + continue; // will reconnect + } + + if ((buf.flags & V4L2_BUF_FLAG_ERROR) == 0) { + DEBUG4("USB " << m_path << ": got image size=" << buf.bytesused + << " index=" << buf.index); + + if (buf.index >= kNumBuffers || !m_buffers[buf.index].m_data) { + WARNING("USB " << m_path << ": invalid buffer" << buf.index); + continue; + } + + PutFrame(static_cast(m_mode.pixelFormat), + llvm::StringRef( + static_cast(m_buffers[buf.index].m_data), + static_cast(buf.bytesused)), + Frame::Time{}); // TODO: time + } + + // Requeue buffer + if (DoIoctl(fd, VIDIOC_QBUF, &buf) != 0) { + WARNING("USB " << m_path << ": could not requeue buffer"); + wasStreaming = m_streaming; + DeviceStreamOff(); + DeviceDisconnect(); + notified = true; // device wasn't deleted, just error'ed + continue; // will reconnect + } + } + } + + DeviceStreamOff(); + + // close camera connection + DeviceDisconnect(); } -void USBCameraImpl::CacheProperty(PropertyData&& prop) const { - // NOTE: must be called with m_mutex held! - int propIndex = ControlIdToProperty(prop.id); - m_properties[prop.name] = propIndex; - m_property_data[propIndex] = std::move(prop); +void USBCameraImpl::DeviceDisconnect() { + int fd = m_fd.exchange(-1); + if (fd < 0) return; // already disconnected + + // Unmap buffers + for (int i = 0; i < kNumBuffers; ++i) + m_buffers[i] = std::move(USBCameraBuffer{}); + + // Close device + close(fd); } -void USBCameraImpl::CacheProperties() const { - int fd = m_fd.load(); +void USBCameraImpl::DeviceConnect() { + if (m_fd >= 0) return; + + INFO("Connecting to USB camera on " << m_path); + + // Try to open the device + DEBUG3("USB " << m_path << ": opening device"); + int fd = open(m_path.c_str(), O_RDWR); if (fd < 0) return; + m_fd = fd; - std::lock_guard lock(m_mutex); - if (m_properties_cached) return; // double-checked locking + // Get capabilities + DEBUG3("USB " << m_path << ": getting capabilities"); + struct v4l2_capability vcap; + std::memset(&vcap, 0, sizeof(vcap)); + if (DoIoctl(fd, VIDIOC_QUERYCAP, &vcap) >= 0) { + m_capabilities = vcap.capabilities; + if (m_capabilities & V4L2_CAP_DEVICE_CAPS) + m_capabilities = vcap.device_caps; + } - constexpr __u32 nextFlags = V4L2_CTRL_FLAG_NEXT_CTRL -#ifdef V4L2_CTRL_FLAG_NEXT_COMPOUND - | V4L2_CTRL_FLAG_NEXT_COMPOUND + // Get or restore video mode + if (!m_properties_cached) { + DEBUG3("USB " << m_path << ": caching properties"); + DeviceCacheMode(); + DeviceCacheProperties(); + DeviceCacheVideoModes(); + m_properties_cached = true; + } else { + DEBUG3("USB " << m_path << ": restoring video mode"); + struct v4l2_format vfmt; + std::memset(&vfmt, 0, sizeof(vfmt)); +#ifdef V4L2_CAP_EXT_PIX_FORMAT + vfmt.fmt.pix.priv = (m_capabilities & V4L2_CAP_EXT_PIX_FORMAT) != 0 + ? V4L2_PIX_FMT_PRIV_MAGIC + : 0; #endif - ; - __u32 id = nextFlags; - PropertyData prop; - - while (ExtCtrlIoctl(fd, &id, &prop) == 0) { - CacheProperty(std::move(prop)); - id |= nextFlags; - } - - if (id == nextFlags) { - // try just enumerating standard... - for (id = V4L2_CID_BASE; id < V4L2_CID_LASTP1; ++id) { - if (ExtCtrlIoctl(fd, &id, &prop) == 0) CacheProperty(std::move(prop)); + vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + switch (m_mode.pixelFormat) { + case VideoMode::kMJPEG: + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; + break; + case VideoMode::kYUYV: + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; + break; + case VideoMode::kRGB565: + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565; + break; + default: + WARNING("USB " << m_path << ": could not set format " + << m_mode.pixelFormat << ", defaulting to MJPEG"); + vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; + break; } - // ... and custom controls - for (id = V4L2_CID_PRIVATE_BASE; ExtCtrlIoctl(fd, &id, &prop) == 0; ++id) { - CacheProperty(std::move(prop)); + vfmt.fmt.pix.width = m_mode.width; + vfmt.fmt.pix.height = m_mode.height; + vfmt.fmt.pix.field = V4L2_FIELD_ANY; + if (DoIoctl(fd, VIDIOC_S_FMT, &vfmt) != 0) { + WARNING("USB " << m_path << ": could not set format " + << m_mode.pixelFormat << " res " << m_mode.width << "x" + << m_mode.height); + } + + // Restore settings + DEBUG3("USB " << m_path << ": restoring settings"); + std::unique_lock lock2(m_mutex); + for (std::size_t i = 0; i < m_propertyData.size(); ++i) { + const auto& prop = m_propertyData[i]; + if (!prop.valueSet) continue; + if (!DeviceSetProperty(lock2, prop)) + WARNING("USB " << m_path << ": failed to set property " << prop.name); } } - m_properties_cached = true; + // Request buffers + DEBUG3("USB " << m_path << ": allocating buffers"); + struct v4l2_requestbuffers rb; + std::memset(&rb, 0, sizeof(rb)); + rb.count = kNumBuffers; + rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + rb.memory = V4L2_MEMORY_MMAP; + if (DoIoctl(fd, VIDIOC_REQBUFS, &rb) != 0) { + WARNING("USB " << m_path << ": could not allocate buffers"); + close(fd); + m_fd = -1; + return; + } + + // Map buffers + DEBUG3("USB " << m_path << ": mapping buffers"); + for (int i = 0; i < kNumBuffers; ++i) { + struct v4l2_buffer buf; + std::memset(&buf, 0, sizeof(buf)); + buf.index = i; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (DoIoctl(fd, VIDIOC_QUERYBUF, &buf) != 0) { + WARNING("USB " << m_path << ": could not query buffer " << i); + close(fd); + m_fd = -1; + return; + } + DEBUG4("USB " << m_path << ": buf " << i << " length=" << buf.length + << " offset=" << buf.m.offset); + + m_buffers[i] = std::move(USBCameraBuffer(fd, buf.length, buf.m.offset)); + if (!m_buffers[i].m_data) { + WARNING("USB " << m_path << ": could not map buffer " << i); + // release other buffers + for (int j = 0; j < i; ++j) m_buffers[j] = std::move(USBCameraBuffer{}); + close(fd); + m_fd = -1; + return; + } + + DEBUG4("USB " << m_path << ": buf " << i + << " address=" << m_buffers[i].m_data); + } } -int USBCameraImpl::GetPropertyIndex(llvm::StringRef name) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - return m_properties.lookup(name); -} +bool USBCameraImpl::DeviceStreamOn() { + if (m_streaming) return false; // ignore if already enabled + int fd = m_fd.load(); + if (fd < 0) return false; -llvm::ArrayRef USBCameraImpl::EnumerateProperties( - llvm::SmallVectorImpl& vec) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - for (const auto& data : m_property_data) vec.push_back(data.getFirst()); - return vec; -} - -bool USBCameraImpl::GetPropertyTypeValueFd(int property, int propType, - unsigned* id, int* type, int* fd, - CS_Status* status) const { - // Get id and type from cached properties - { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; + // Queue buffers + DEBUG3("USB " << m_path << ": queuing buffers"); + for (int i = 0; i < kNumBuffers; ++i) { + struct v4l2_buffer buf; + std::memset(&buf, 0, sizeof(buf)); + buf.index = i; + buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + buf.memory = V4L2_MEMORY_MMAP; + if (DoIoctl(fd, VIDIOC_QBUF, &buf) != 0) { + WARNING("USB " << m_path << ": could not queue buffer " << i); return false; } - if ((it->getSecond().propType & propType) == 0) { - *status = CS_WRONG_PROPERTY_TYPE; - return false; - } - *id = it->getSecond().id; - *type = it->getSecond().type; } - *fd = m_fd.load(); - if (*fd < 0) { - *status = CS_SOURCE_IS_DISCONNECTED; - return false; - } + // Turn stream on + int type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (DoIoctl(fd, VIDIOC_STREAMON, &type) != 0) return false; + DEBUG4("USB " << m_path << ": enabled streaming"); + m_streaming = true; return true; } -CS_PropertyType USBCameraImpl::GetPropertyType(int property) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) return CS_PROP_NONE; - return it->getSecond().propType; +bool USBCameraImpl::DeviceStreamOff() { + if (!m_streaming) return false; // ignore if already disabled + int fd = m_fd.load(); + if (fd < 0) return false; + int type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (DoIoctl(fd, VIDIOC_STREAMOFF, &type) != 0) return false; + DEBUG4("USB " << m_path << ": disabled streaming"); + m_streaming = false; + return true; } -llvm::StringRef USBCameraImpl::GetPropertyName(int property, - llvm::SmallVectorImpl& buf, - CS_Status* status) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; - return llvm::StringRef{}; +void USBCameraImpl::DeviceProcessCommands() { + std::unique_lock lock(m_mutex); + if (m_commands.empty()) return; + while (!m_commands.empty()) { + auto msg = std::move(m_commands.back()); + m_commands.pop_back(); + + if (msg->type == Message::kCmdSetMode || + msg->type == Message::kCmdSetPixelFormat || + msg->type == Message::kCmdSetResolution || + msg->type == Message::kCmdSetFPS) { + VideoMode newMode; + if (msg->type == Message::kCmdSetMode) { + newMode.pixelFormat = msg->data[0]; + newMode.width = msg->data[1]; + newMode.height = msg->data[2]; + newMode.fps = msg->data[3]; + m_modeSetPixelFormat = true; + m_modeSetResolution = true; + m_modeSetFPS = true; + } else if (msg->type == Message::kCmdSetPixelFormat) { + newMode = m_mode; + newMode.pixelFormat = msg->data[0]; + m_modeSetPixelFormat = true; + } else if (msg->type == Message::kCmdSetResolution) { + newMode = m_mode; + newMode.width = msg->data[0]; + newMode.height = msg->data[1]; + m_modeSetResolution = true; + } else if (msg->type == Message::kCmdSetFPS) { + newMode = m_mode; + newMode.fps = msg->data[0]; + m_modeSetFPS = true; + } + + // If the resolution changed, we need to disconnect and reconnect + if (newMode.width != m_mode.width || newMode.height != m_mode.height) { + lock.unlock(); + bool wasStreaming = m_streaming; + if (wasStreaming) DeviceStreamOff(); + m_mode = newMode; + if (m_fd >= 0) { + DeviceDisconnect(); + DeviceConnect(); + DeviceSetFPS(); + } + if (wasStreaming) DeviceStreamOn(); + lock.lock(); + } else { + // TODO + m_mode = newMode; + } + } else if (msg->type == Message::kCmdSetProperty || + msg->type == Message::kCmdSetPropertyStr) { + int property = msg->data[0]; + + // Look up + auto prop = GetProperty(property); + if (!prop) { + msg->type = Message::kError; + msg->data[0] = CS_INVALID_PROPERTY; + goto done; + } + + // Check type match + if ((msg->type == Message::kCmdSetPropertyStr && + prop->propType != CS_PROP_STRING) || + (msg->type == Message::kCmdSetProperty && + (prop->propType & + (CS_PROP_BOOLEAN | CS_PROP_INTEGER | CS_PROP_ENUM)) == 0)) { + msg->type = Message::kError; + msg->data[0] = CS_WRONG_PROPERTY_TYPE; + goto done; + } + + // If we're connected... + int fd = m_fd.load(); + if (fd >= 0) { + // Get into local variables before we release the lock + int rv; + unsigned id = prop->id; + int maximum = prop->maximum; + int type = prop->type; + + // Set the property value on the device + lock.unlock(); + if (msg->type == Message::kCmdSetPropertyStr) + rv = SetStringCtrlIoctl(fd, id, maximum, msg->dataStr); + else + rv = + SetIntCtrlIoctl(fd, id, type, static_cast(msg->data[1])); + lock.lock(); + + if (rv < 0) { + msg->type = Message::kError; + msg->data[0] = CS_PROPERTY_WRITE_FAILED; + goto done; + } + } + + // Cache the set value. We need to re-get the pointer due to + // releasing the lock. + prop = GetProperty(property); + if (msg->type == Message::kCmdSetPropertyStr) + prop->valueStr = msg->dataStr; + else + prop->value = msg->data[1]; + prop->valueSet = true; + msg->type = Message::kOk; + } else if (msg->type == Message::kNumSinksChanged || + msg->type == Message::kNumSinksEnabledChanged) { + // These are send-only messages, so recycle here. DestroyMessage needs + // the mutex, so unlock it. We don't actually do anything directly + // based on these messages (instead we check in the main loop), but + // they do wake up the thread. + lock.unlock(); + DestroyMessage(std::move(msg)); + lock.lock(); + } else { + msg->type = Message::kNone; + } + +done: + if (msg) m_responses.emplace_back(std::move(msg)); } - return it->getSecond().name; // safe because we never modify it after caching + lock.unlock(); + m_responseCv.notify_all(); } -int USBCameraImpl::GetProperty(int property, CS_Status* status) const { - unsigned id; - int type; - int fd; - if (!GetPropertyTypeValueFd(property, - CS_PROP_BOOLEAN | CS_PROP_INTEGER | CS_PROP_ENUM, - &id, &type, &fd, status)) - return false; +void USBCameraImpl::DeviceSetFPS() { + int fd = m_fd.load(); + if (fd < 0) return; - int64_t value = 0; - if (GetIntCtrlIoctl(fd, id, type, &value) < 0) { - *status = CS_READ_FAILED; - return false; - } - return value; -} - -void USBCameraImpl::SetProperty(int property, int value, CS_Status* status) { - unsigned id; - int type; - int fd; - if (!GetPropertyTypeValueFd(property, - CS_PROP_BOOLEAN | CS_PROP_INTEGER | CS_PROP_ENUM, - &id, &type, &fd, status)) - return; - - if (SetIntCtrlIoctl(fd, id, type, static_cast(value)) < 0) { - *status = CS_PROPERTY_WRITE_FAILED; - } -} - -int USBCameraImpl::GetPropertyMin(int property, CS_Status* status) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; - return 0; - } - return it->getSecond().minimum; -} - -int USBCameraImpl::GetPropertyMax(int property, CS_Status* status) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; - return 0; - } - return it->getSecond().maximum; -} - -int USBCameraImpl::GetPropertyStep(int property, CS_Status* status) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; - return 0; - } - return it->getSecond().step; -} - -int USBCameraImpl::GetPropertyDefault(int property, CS_Status* status) const { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; - return 0; - } - return it->getSecond().defaultValue; -} - -llvm::StringRef USBCameraImpl::GetStringProperty( - int property, llvm::SmallVectorImpl& buf, CS_Status* status) const { - unsigned id; - int type; - int fd; - if (!GetPropertyTypeValueFd(property, CS_PROP_STRING, &id, &type, &fd, - status)) - return llvm::StringRef{}; - - struct v4l2_ext_control ctrl; - struct v4l2_ext_controls ctrls; - std::memset(&ctrl, 0, sizeof(ctrl)); - ctrl.id = id; - ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(id); - ctrls.count = 1; - ctrls.controls = &ctrl; - int rc = DoIoctl(fd, VIDIOC_G_EXT_CTRLS, &ctrls); - if (rc < 0) { - *status = CS_READ_FAILED; - return llvm::StringRef{}; - } - buf.append(ctrl.string, ctrl.string + std::strlen(ctrl.string)); - return llvm::StringRef(buf.data(), buf.size()); -} - -void USBCameraImpl::SetStringProperty(int property, llvm::StringRef value, - CS_Status* status) { - unsigned id; - std::size_t maximum; - // Get id and maximum from cached properties + int fps; { - if (!m_properties_cached) CacheProperties(); std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; - return; - } - if (it->getSecond().propType != CS_PROP_STRING) { - *status = CS_WRONG_PROPERTY_TYPE; - return; - } - id = it->getSecond().id; - maximum = static_cast(it->getSecond().maximum); - } - - int fd = m_fd.load(); - if (fd < 0) { - *status = CS_SOURCE_IS_DISCONNECTED; - return; - } - - llvm::SmallString<64> str{value.substr(0, std::min(value.size(), maximum))}; - - struct v4l2_ext_control ctrl; - struct v4l2_ext_controls ctrls; - std::memset(&ctrl, 0, sizeof(ctrl)); - ctrl.id = id; - ctrl.size = str.size(); - ctrl.string = const_cast(str.c_str()); - ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(id); - ctrls.count = 1; - ctrls.controls = &ctrl; - int rc = DoIoctl(fd, VIDIOC_S_EXT_CTRLS, &ctrls); - if (rc < 0) { - *status = CS_PROPERTY_WRITE_FAILED; - return; + fps = m_mode.fps; } + struct v4l2_streamparm parm; + std::memset(&parm, 0, sizeof(parm)); + parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + if (DoIoctl(fd, VIDIOC_G_PARM, &parm) != 0) return; + if ((parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) == 0) return; + std::memset(&parm, 0, sizeof(parm)); + parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; + parm.parm.capture.timeperframe = FPSToFract(fps); + if (DoIoctl(fd, VIDIOC_S_PARM, &parm) != 0) return; } -std::vector USBCameraImpl::GetEnumPropertyChoices( - int property, CS_Status* status) const { - unsigned id; - unsigned minimum; - unsigned maximum; - // Get id, minimum, and maximum from cached properties - { - if (!m_properties_cached) CacheProperties(); - std::lock_guard lock(m_mutex); - auto it = m_property_data.find(property); - if (it == m_property_data.end()) { - *status = CS_INVALID_PROPERTY; - return std::vector{}; - } - if (it->getSecond().propType != CS_PROP_ENUM) { - *status = CS_WRONG_PROPERTY_TYPE; - return std::vector{}; - } - id = it->getSecond().id; - minimum = static_cast(it->getSecond().minimum); - maximum = static_cast(it->getSecond().maximum); - } - +void USBCameraImpl::DeviceCacheMode() { int fd = m_fd.load(); - if (fd < 0) { - *status = CS_SOURCE_IS_DISCONNECTED; - return std::vector{}; - } - - std::vector vec(maximum + 1); - v4l2_querymenu qmenu; - std::memset(&qmenu, 0, sizeof(qmenu)); - qmenu.id = id; - for (unsigned i = minimum; i <= maximum; ++i) { - qmenu.index = static_cast<__u32>(i); - if (TryIoctl(fd, VIDIOC_QUERYMENU, &qmenu) != 0) continue; - vec[i] = reinterpret_cast(qmenu.name); - } - - return vec; -} - -VideoMode USBCameraImpl::GetVideoMode(CS_Status* status) const { - int fd = m_fd.load(); - if (fd < 0) { - *status = CS_SOURCE_IS_DISCONNECTED; - return VideoMode{}; - } + if (fd < 0) return; // Get format struct v4l2_format vfmt; @@ -642,8 +916,10 @@ VideoMode USBCameraImpl::GetVideoMode(CS_Status* status) const { #endif vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; if (DoIoctl(fd, VIDIOC_G_FMT, &vfmt) != 0) { - *status = CS_READ_FAILED; - return VideoMode{}; + ERROR("USB " << m_path << ": could not read current video mode"); + std::lock_guard lock(m_mutex); + m_mode = VideoMode{VideoMode::kMJPEG, 320, 240, 30}; + return; } VideoMode::PixelFormat pixelFormat; switch (vfmt.fmt.pix.pixelformat) { @@ -671,85 +947,102 @@ VideoMode USBCameraImpl::GetVideoMode(CS_Status* status) const { fps = FractToFPS(parm.parm.capture.timeperframe); } - VideoMode mode(pixelFormat, vfmt.fmt.pix.width, vfmt.fmt.pix.height, fps); - - // Re-cache + // Save std::lock_guard lock(m_mutex); - m_mode = mode; - return mode; + m_mode.pixelFormat = pixelFormat; + m_mode.width = vfmt.fmt.pix.width; + m_mode.height = vfmt.fmt.pix.height; + m_mode.fps = fps; } -bool USBCameraImpl::SetVideoModePixRes(const VideoMode& mode, - CS_Status* status) { - int fd = m_fd.load(); - if (fd < 0) { - *status = CS_SOURCE_IS_DISCONNECTED; - return false; +void USBCameraImpl::DeviceCacheProperty(PropertyData&& prop) { + std::unique_lock lock(m_mutex); + int& ndx = m_properties[prop.name]; + if (ndx == 0) { + // get the value + lock.unlock(); + if (!DeviceGetProperty(&prop)) + WARNING("USB " << m_path << ": failed to get property " << prop.name); + lock.lock(); + // create a new index + ndx = m_propertyData.size() + 1; + m_propertyData.emplace_back(std::move(prop)); + } else { + // merge with existing settings + auto* prop2 = GetProperty(ndx); + prop.valueSet = prop2->valueSet; + prop.value = prop2->value; + prop.valueStr = std::move(prop2->valueStr); + lock.unlock(); + if (prop.valueSet) { + // set the value if it was previously set + if (!DeviceSetProperty(lock, prop)) + WARNING("USB " << m_path << ": failed to set property " << prop.name); + } else { + // otherwise get the value + if (!DeviceGetProperty(&prop)) + WARNING("USB " << m_path << ": failed to get property " << prop.name); + } + lock.lock(); + // need to re-get as we unlocked + *GetProperty(ndx) = std::move(prop); } +} - struct v4l2_format vfmt; - std::memset(&vfmt, 0, sizeof(vfmt)); -#ifdef V4L2_CAP_EXT_PIX_FORMAT - vfmt.fmt.pix.priv = (m_capabilities & V4L2_CAP_EXT_PIX_FORMAT) != 0 - ? V4L2_PIX_FMT_PRIV_MAGIC - : 0; +void USBCameraImpl::DeviceCacheProperties() { + int fd = m_fd.load(); + if (fd < 0) return; + + constexpr __u32 nextFlags = V4L2_CTRL_FLAG_NEXT_CTRL +#ifdef V4L2_CTRL_FLAG_NEXT_COMPOUND + | V4L2_CTRL_FLAG_NEXT_COMPOUND #endif - vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - switch (mode.pixelFormat) { - case VideoMode::kMJPEG: - vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG; - break; - case VideoMode::kYUYV: - vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; - break; - case VideoMode::kRGB565: - vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565; - break; - default: - return false; - } - vfmt.fmt.pix.width = mode.width; - vfmt.fmt.pix.height = mode.height; + ; + __u32 id = nextFlags; + PropertyData prop; - if (DoIoctl(fd, VIDIOC_S_FMT, &vfmt) != 0) return false; - return true; + while (ExtCtrlIoctl(fd, &id, &prop) == 0) { + DeviceCacheProperty(std::move(prop)); + id |= nextFlags; + } + + if (id == nextFlags) { + // try just enumerating standard... + for (id = V4L2_CID_BASE; id < V4L2_CID_LASTP1; ++id) { + if (ExtCtrlIoctl(fd, &id, &prop) == 0) + DeviceCacheProperty(std::move(prop)); + } + // ... and custom controls + for (id = V4L2_CID_PRIVATE_BASE; ExtCtrlIoctl(fd, &id, &prop) == 0; ++id) + DeviceCacheProperty(std::move(prop)); + } } -bool USBCameraImpl::SetVideoMode(const VideoMode& mode, CS_Status* status) { - if (!SetVideoModePixRes(mode, status)) return false; - if (!SetFPS(mode.fps, status)) return false; - { - std::lock_guard lock(m_mutex); - m_mode = mode; - } - m_mode_changed.notify_one(); - return true; -} - -std::vector USBCameraImpl::EnumerateVideoModes( - CS_Status* status) const { - std::vector rv; +void USBCameraImpl::DeviceCacheVideoModes() { int fd = m_fd.load(); - if (fd < 0) { - *status = CS_SOURCE_IS_DISCONNECTED; - return rv; - } + if (fd < 0) return; - // Formats + std::vector modes; + + // Pixel formats struct v4l2_fmtdesc fmt; - + std::memset(&fmt, 0, sizeof(fmt)); fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; for (fmt.index = 0; TryIoctl(fd, VIDIOC_ENUM_FMT, &fmt) >= 0; ++fmt.index) { VideoMode::PixelFormat pixelFormat = ToPixelFormat(fmt.pixelformat); if (pixelFormat == VideoMode::kUnknown) continue; + // Frame sizes struct v4l2_frmsizeenum frmsize; + std::memset(&frmsize, 0, sizeof(frmsize)); frmsize.pixel_format = fmt.pixelformat; for (frmsize.index = 0; TryIoctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) >= 0; ++frmsize.index) { if (frmsize.type != V4L2_FRMSIZE_TYPE_DISCRETE) continue; + // Frame intervals struct v4l2_frmivalenum frmival; + std::memset(&frmival, 0, sizeof(frmival)); frmival.pixel_format = fmt.pixelformat; frmival.width = frmsize.discrete.width; frmival.height = frmsize.discrete.height; @@ -758,86 +1051,405 @@ std::vector USBCameraImpl::EnumerateVideoModes( ++frmival.index) { if (frmival.type != V4L2_FRMIVAL_TYPE_DISCRETE) continue; - rv.emplace_back(pixelFormat, static_cast(frmsize.discrete.width), - static_cast(frmsize.discrete.height), - FractToFPS(frmival.discrete)); + modes.emplace_back(pixelFormat, + static_cast(frmsize.discrete.width), + static_cast(frmsize.discrete.height), + FractToFPS(frmival.discrete)); } } } + std::lock_guard lock(m_mutex); + m_videoModes.swap(modes); +} + +bool USBCameraImpl::DeviceGetProperty(PropertyData* prop) { + int fd = m_fd.load(); + if (fd < 0) return true; + int rv = 0; + + switch (prop->propType) { + case CS_PROP_BOOLEAN: + case CS_PROP_INTEGER: + case CS_PROP_ENUM: + { + int64_t value = 0; + rv = GetIntCtrlIoctl(fd, prop->id, prop->type, &value); + if (rv >= 0) prop->value = value; + } + break; + case CS_PROP_STRING: + rv = GetStringCtrlIoctl(fd, prop->id, prop->maximum, &prop->valueStr); + break; + default: + break; + } + + return rv >= 0; +} + +bool USBCameraImpl::DeviceSetProperty(std::unique_lock& lock, + const PropertyData& prop) { + int fd = m_fd.load(); + if (fd < 0) return true; + unsigned id = prop.id; + int rv = 0; + + switch (prop.propType) { + case CS_PROP_BOOLEAN: + case CS_PROP_INTEGER: + case CS_PROP_ENUM: + { + int type = prop.type; + int value = prop.value; + lock.unlock(); + rv = SetIntCtrlIoctl(fd, id, type, value); + lock.lock(); + } + break; + case CS_PROP_STRING: + { + int maximum = prop.maximum; + llvm::SmallString<128> valueStr{prop.valueStr}; + lock.unlock(); + rv = SetStringCtrlIoctl(fd, id, maximum, valueStr); + lock.lock(); + } + break; + default: + break; + } + + return rv >= 0; +} + +std::unique_ptr USBCameraImpl::SendAndWait( + std::unique_ptr msg) const { + int fd = m_command_fd.load(); + if (fd < 0) { + // not possible to signal, exit early + DestroyMessage(std::move(msg)); + return nullptr; + } + + // Use the message address as a unique identifier + Message* ptr = msg.get(); + + // Add the message to the command queue + { + std::lock_guard lock(m_mutex); + m_commands.emplace_back(std::move(msg)); + } + + // Signal the camera thread + if (eventfd_write(fd, 1) < 0) return nullptr; + + std::unique_lock lock(m_mutex); + while (m_active) { + // Did we get a response to *our* request? + auto it = std::find_if( + m_responses.begin(), m_responses.end(), + [=](const std::unique_ptr& m) { return m.get() == ptr; }); + if (it != m_responses.end()) { + // Yes, remove it from the vector and we're done. + auto rv = std::move(*it); + m_responses.erase(it); + return rv; + } + // No, keep waiting for a response + m_responseCv.wait(lock); + } + return nullptr; +} + +void USBCameraImpl::Send(std::unique_ptr msg) const { + int fd = m_command_fd.load(); + if (fd < 0) { + // not possible to signal, exit early + DestroyMessage(std::move(msg)); + return; + } + + // Add the message to the command queue + { + std::lock_guard lock(m_mutex); + m_commands.emplace_back(std::move(msg)); + } + + // Signal the camera thread + eventfd_write(fd, 1); +} + +bool USBCameraImpl::CacheProperties(CS_Status* status) const { + // Wake up camera thread; this will try to reconnect + auto msg = CreateMessage(Message::kNone); + msg = std::move(SendAndWait(std::move(msg))); + if (!msg) { + *status = CS_SOURCE_IS_DISCONNECTED; + return false; + } + DestroyMessage(std::move(msg)); + if (!m_properties_cached) { + *status = CS_SOURCE_IS_DISCONNECTED; + return false; + } + return true; +} + +llvm::StringRef USBCameraImpl::GetDescription( + llvm::SmallVectorImpl& buf) const { + return m_description; +} + +bool USBCameraImpl::IsConnected() const { return m_fd >= 0; } + +int USBCameraImpl::GetPropertyIndex(llvm::StringRef name) const { + // We can't fail, so instead we create a new index if caching fails. + CS_Status status = 0; + if (!m_properties_cached) CacheProperties(&status); + std::lock_guard lock(m_mutex); + int& ndx = m_properties[name]; + if (ndx == 0) { + // create a new index + ndx = m_propertyData.size() + 1; + m_propertyData.emplace_back(); + } + return ndx; +} + +llvm::ArrayRef USBCameraImpl::EnumerateProperties( + llvm::SmallVectorImpl& vec, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) + return llvm::ArrayRef{}; + std::lock_guard lock(m_mutex); + for (int i = 0; i < static_cast(m_propertyData.size()); ++i) + vec.push_back(i + 1); + return vec; +} + +CS_PropertyType USBCameraImpl::GetPropertyType(int property) const { + CS_Status status = 0; + if (!m_properties_cached && !CacheProperties(&status)) return CS_PROP_NONE; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) return CS_PROP_NONE; + return prop->propType; +} + +llvm::StringRef USBCameraImpl::GetPropertyName(int property, + llvm::SmallVectorImpl& buf, + CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) + return llvm::StringRef{}; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return llvm::StringRef{}; + } + // safe to not copy because we never modify it after caching + return prop->name; +} + +int USBCameraImpl::GetProperty(int property, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) return 0; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return 0; + } + if ((prop->propType & (CS_PROP_BOOLEAN | CS_PROP_INTEGER | CS_PROP_ENUM)) == + 0) { + *status = CS_WRONG_PROPERTY_TYPE; + return 0; + } + return prop->value; +} + +void USBCameraImpl::SetProperty(int property, int value, CS_Status* status) { + auto msg = CreateMessage(Message::kCmdSetProperty); + msg->data[0] = property; + msg->data[1] = value; + msg = std::move(SendAndWait(std::move(msg))); + if (!msg) return; + if (msg->type == Message::kError) *status = msg->data[0]; + DestroyMessage(std::move(msg)); +} + +int USBCameraImpl::GetPropertyMin(int property, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) return 0; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return 0; + } + return prop->minimum; +} + +int USBCameraImpl::GetPropertyMax(int property, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) return 0; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return 0; + } + return prop->maximum; +} + +int USBCameraImpl::GetPropertyStep(int property, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) return 0; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return 0; + } + return prop->step; +} + +int USBCameraImpl::GetPropertyDefault(int property, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) return 0; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return 0; + } + return prop->defaultValue; +} + +llvm::StringRef USBCameraImpl::GetStringProperty( + int property, llvm::SmallVectorImpl& buf, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) + return llvm::StringRef{}; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return llvm::StringRef{}; + } + if (prop->propType != CS_PROP_STRING) { + *status = CS_WRONG_PROPERTY_TYPE; + return llvm::StringRef{}; + } + buf.clear(); + buf.append(prop->valueStr.begin(), prop->valueStr.end()); + return llvm::StringRef(buf.data(), buf.size()); +} + +void USBCameraImpl::SetStringProperty(int property, llvm::StringRef value, + CS_Status* status) { + auto msg = CreateMessage(Message::kCmdSetPropertyStr); + msg->data[0] = property; + msg->dataStr = value; + msg = std::move(SendAndWait(std::move(msg))); + if (!msg) return; + if (msg->type == Message::kError) *status = msg->data[0]; + DestroyMessage(std::move(msg)); +} + +std::vector USBCameraImpl::GetEnumPropertyChoices( + int property, CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) + return std::vector{}; + std::lock_guard lock(m_mutex); + auto prop = GetProperty(property); + if (!prop) { + *status = CS_INVALID_PROPERTY; + return std::vector{}; + } + if (prop->propType != CS_PROP_ENUM) { + *status = CS_WRONG_PROPERTY_TYPE; + return std::vector{}; + } + return prop->enumChoices; +} + +VideoMode USBCameraImpl::GetVideoMode(CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) + return VideoMode{}; + std::lock_guard lock(m_mutex); + return m_mode; +} + +bool USBCameraImpl::SetVideoMode(const VideoMode& mode, CS_Status* status) { + auto msg = CreateMessage(Message::kCmdSetMode); + msg->data[0] = mode.pixelFormat; + msg->data[1] = mode.width; + msg->data[2] = mode.height; + msg->data[3] = mode.fps; + msg = std::move(SendAndWait(std::move(msg))); + if (!msg) return false; + bool rv = true; + if (msg->type == Message::kError) { + *status = msg->data[0]; + rv = false; + } + DestroyMessage(std::move(msg)); return rv; } bool USBCameraImpl::SetPixelFormat(VideoMode::PixelFormat pixelFormat, CS_Status* status) { - // Copy cached mode so we don't hold lock during ioctl - VideoMode mode; - { - std::lock_guard lock(m_mutex); - mode = m_mode; + auto msg = CreateMessage(Message::kCmdSetPixelFormat); + msg->data[0] = pixelFormat; + msg = std::move(SendAndWait(std::move(msg))); + if (!msg) return false; + bool rv = true; + if (msg->type == Message::kError) { + *status = msg->data[0]; + rv = false; } - mode.pixelFormat = pixelFormat; - if (!SetVideoModePixRes(mode, status)) return false; - { - std::lock_guard lock(m_mutex); - m_mode.pixelFormat = pixelFormat; - } - m_mode_changed.notify_one(); - return true; + DestroyMessage(std::move(msg)); + return rv; } bool USBCameraImpl::SetResolution(int width, int height, CS_Status* status) { - // Copy cached mode so we don't hold lock during ioctl - VideoMode mode; - { - std::lock_guard lock(m_mutex); - mode = m_mode; + auto msg = CreateMessage(Message::kCmdSetResolution); + msg->data[0] = width; + msg->data[1] = height; + msg = std::move(SendAndWait(std::move(msg))); + if (!msg) return false; + bool rv = true; + if (msg->type == Message::kError) { + *status = msg->data[0]; + rv = false; } - mode.width = width; - mode.height = height; - if (!SetVideoModePixRes(mode, status)) return false; - { - std::lock_guard lock(m_mutex); - m_mode.width = width; - m_mode.height = height; - } - m_mode_changed.notify_one(); - return true; + DestroyMessage(std::move(msg)); + return rv; } bool USBCameraImpl::SetFPS(int fps, CS_Status* status) { - int fd = m_fd.load(); - if (fd < 0) { - *status = CS_SOURCE_IS_DISCONNECTED; - return false; + auto msg = CreateMessage(Message::kCmdSetFPS); + msg->data[0] = fps; + msg = std::move(SendAndWait(std::move(msg))); + if (!msg) return false; + bool rv = true; + if (msg->type == Message::kError) { + *status = msg->data[0]; + rv = false; } - - struct v4l2_streamparm parm; - std::memset(&parm, 0, sizeof(parm)); - parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - if (DoIoctl(fd, VIDIOC_G_PARM, &parm) != 0) return false; - if ((parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) == 0) return false; - std::memset(&parm, 0, sizeof(parm)); - parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE; - parm.parm.capture.timeperframe = FPSToFract(fps); - if (DoIoctl(fd, VIDIOC_S_PARM, &parm) != 0) return false; - - { - std::lock_guard lock(m_mutex); - m_mode.fps = fps; - } - m_mode_changed.notify_one(); - return true; + DestroyMessage(std::move(msg)); + return rv; } -void USBCameraImpl::Stop() { - m_active = false; +std::vector USBCameraImpl::EnumerateVideoModes( + CS_Status* status) const { + if (!m_properties_cached && !CacheProperties(status)) + return std::vector{}; + std::lock_guard lock(m_mutex); + return m_videoModes; +} - // close camera connection - int fd = m_fd.exchange(-1); - if (fd >= 0) close(fd); +void USBCameraImpl::NumSinksChanged() { + Send(CreateMessage(Message::kNumSinksChanged)); +} - // join camera thread +void USBCameraImpl::NumSinksEnabledChanged() { + Send(CreateMessage(Message::kNumSinksEnabledChanged)); } namespace cs { diff --git a/src/USBCameraImpl.h b/src/USBCameraImpl.h index 6c6046a4e6..ace4cf27b2 100644 --- a/src/USBCameraImpl.h +++ b/src/USBCameraImpl.h @@ -10,18 +10,20 @@ #include #include +#include #ifdef __linux__ #include #endif #include "llvm/raw_ostream.h" -#include "llvm/DenseMap.h" #include "llvm/SmallVector.h" #include "llvm/StringMap.h" +#include "llvm/STLExtras.h" #include "support/raw_istream.h" #include "SourceImpl.h" +#include "USBCameraBuffer.h" namespace cs { @@ -32,11 +34,12 @@ class USBCameraImpl : public SourceImpl { llvm::StringRef GetDescription( llvm::SmallVectorImpl& buf) const override; + bool IsConnected() const override; // Property functions int GetPropertyIndex(llvm::StringRef name) const override; - llvm::ArrayRef EnumerateProperties( - llvm::SmallVectorImpl& vec) const override; + llvm::ArrayRef EnumerateProperties(llvm::SmallVectorImpl& vec, + CS_Status* status) const override; CS_PropertyType GetPropertyType(int property) const override; llvm::StringRef GetPropertyName(int property, llvm::SmallVectorImpl& buf, @@ -57,14 +60,16 @@ class USBCameraImpl : public SourceImpl { VideoMode GetVideoMode(CS_Status* status) const override; bool SetVideoMode(const VideoMode& mode, CS_Status* status) override; - std::vector EnumerateVideoModes(CS_Status* status) const override; bool SetPixelFormat(VideoMode::PixelFormat pixelFormat, CS_Status* status) override; bool SetResolution(int width, int height, CS_Status* status) override; bool SetFPS(int fps, CS_Status* status) override; + std::vector EnumerateVideoModes(CS_Status* status) const override; - void Stop(); + void NumSinksChanged() override; + void NumSinksEnabledChanged() override; + // Property data struct PropertyData { PropertyData() = default; #ifdef __linux__ @@ -77,41 +82,142 @@ class USBCameraImpl : public SourceImpl { std::string name; unsigned id; // implementation-level id int type; // implementation type, not CS_PropertyType! - CS_PropertyType propType; + CS_PropertyType propType{CS_PROP_NONE}; int minimum; int maximum; int step; int defaultValue; + int value{0}; + std::string valueStr; + std::vector enumChoices; + bool valueSet{false}; + }; + + // Messages passed to/from camera thread + struct Message { + enum Type { + kNone = 0, + kCmdSetMode, + kCmdSetPixelFormat, + kCmdSetResolution, + kCmdSetFPS, + kCmdSetProperty, + kCmdSetPropertyStr, + kNumSinksChanged, // no response + kNumSinksEnabledChanged, // no response + // Responses + kOk, + kError + }; + + Type type; + int data[4]; + std::string dataStr; }; private: - mutable llvm::DenseMap m_property_data; - mutable llvm::StringMap m_properties; - mutable std::atomic_bool m_properties_cached{false}; + // Message pool access + std::unique_ptr CreateMessage(Message::Type type) const { + std::lock_guard lock(m_mutex); + if (m_messagePool.empty()) return llvm::make_unique(); + auto rv = std::move(m_messagePool.back()); + m_messagePool.pop_back(); + rv->type = type; + return rv; + } + void DestroyMessage(std::unique_ptr message) const { + std::lock_guard lock(m_mutex); + m_messagePool.emplace_back(std::move(message)); + } - void CacheProperty(PropertyData&& prop) const; - void CacheProperties() const; - bool GetPropertyTypeValueFd(int property, int propType, unsigned* id, - int* type, int* fd, CS_Status* status) const; - bool SetVideoModePixRes(const VideoMode& mode, CS_Status* status); + // Send a message to the camera thread and wait for a response (generic) + std::unique_ptr SendAndWait(std::unique_ptr msg) const; + // Send a message to the camera thread with no response + void Send(std::unique_ptr msg) const; + // Cache properties. Immediately successful if properties are already cached. + // If they are not, tries to connect to the camera to do so; returns false and + // sets status to CS_SOURCE_IS_DISCONNECTED if that too fails. + bool CacheProperties(CS_Status* status) const; + + // The camera processing thread void CameraThreadMain(); - std::string m_path; - std::string m_description; + // Functions used by CameraThreadMain() + void DeviceDisconnect(); + void DeviceConnect(); + bool DeviceStreamOn(); + bool DeviceStreamOff(); + void DeviceProcessCommands(); + void DeviceSetFPS(); + void DeviceCacheMode(); + void DeviceCacheProperty(PropertyData&& prop); + void DeviceCacheProperties(); + void DeviceCacheVideoModes(); + bool DeviceGetProperty(PropertyData* prop); + bool DeviceSetProperty(std::unique_lock& lock, + const PropertyData& prop); - std::atomic_int m_fd; - - std::atomic_bool m_active; // set to false to terminate threads - std::thread m_cameraThread; - - mutable VideoMode m_mode; + // + // Variables only used within camera thread + // + bool m_streaming; + bool m_modeSetPixelFormat{false}; + bool m_modeSetResolution{false}; + bool m_modeSetFPS{false}; #ifdef __linux__ unsigned m_capabilities = 0; #endif + // Number of buffers to ask OS for + static constexpr int kNumBuffers = 4; + std::array m_buffers; + + // + // Path and description: These never change, so not protected by mutex. + // + std::string m_path; + std::string m_description; + +#ifdef __linux__ + std::atomic_int m_fd; + std::atomic_int m_command_fd; // for command eventfd +#endif + + std::atomic_bool m_active; // set to false to terminate thread + std::thread m_cameraThread; + + // + // Variables protected by m_mutex + // + + // Cached camera information (properties and video modes) + mutable std::vector m_propertyData; + mutable llvm::StringMap m_properties; + std::vector m_videoModes; + std::atomic_bool m_properties_cached{false}; + + // Get a property; must be called with m_mutex held. + PropertyData* GetProperty(int property) { + if (property <= 0 || static_cast(property) > m_propertyData.size()) + return nullptr; + return &m_propertyData[property - 1]; + } + const PropertyData* GetProperty(int property) const { + if (property <= 0 || static_cast(property) > m_propertyData.size()) + return nullptr; + return &m_propertyData[property - 1]; + } + + // Current video mode (updated by camera thread) + VideoMode m_mode; + + // Message pool and queues + mutable std::vector> m_messagePool; + mutable std::vector> m_commands; + mutable std::vector> m_responses; + mutable std::condition_variable m_responseCv; mutable std::mutex m_mutex; - mutable std::condition_variable m_mode_changed; }; } // namespace cs diff --git a/src/cameraserver_cpp.cpp b/src/cameraserver_cpp.cpp index aa92e863a1..8800cfd5ad 100644 --- a/src/cameraserver_cpp.cpp +++ b/src/cameraserver_cpp.cpp @@ -234,7 +234,8 @@ llvm::ArrayRef EnumerateSourceProperties( return 0; } llvm::SmallVector properties_buf; - for (auto property : data->source->EnumerateProperties(properties_buf)) + for (auto property : + data->source->EnumerateProperties(properties_buf, status)) vec.push_back(Handle{source, property, Handle::kProperty}); return vec; }