Finish most of USBCameraImpl.

The main thing not yet fully implemented is video mode setting.

Also fix a handful of bugs in HTTPSinkImpl.

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<Data>(m_data));
+  m_data = nullptr;
+}

src/Frame.h

@@ -10,10 +10,11 @@
 
 #include <atomic>
 #include <chrono>
+#include <memory>
 
-#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> 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 {

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&param2=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<int, 32> 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";

src/Image.h

@@ -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 <cstddef>
-
-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_

src/SourceImpl.cpp

@@ -7,18 +7,28 @@
 
 #include "SourceImpl.h"
 
-#include "llvm/STLExtras.h"
+#include <algorithm>
+#include <cstring>
+
+#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<std::mutex> lock{m_mutex};
-  if (m_frameData) return;
-  if (m_framesAvail.empty()) {
-    m_frameData = llvm::make_unique<Frame::Data>();
-  } 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<Frame::Data> frameData;
   {
     std::lock_guard<std::mutex> lock{m_mutex};
-    std::lock_guard<std::mutex> 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<std::mutex> 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<Frame::Data> data) {
   std::lock_guard<std::mutex> 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<Frame::Data>& a,
+                                   const std::unique_ptr<Frame::Data>& b) {
+                                  return a->capacity < b->capacity;
+                                });
+    if ((*it2)->capacity < data->capacity)
+      *it2 = std::move(data);
+  } else
+    m_framesAvail.emplace_back(std::move(data));
 }

src/SourceImpl.h

@@ -34,38 +34,35 @@ class SourceImpl {
   llvm::StringRef GetName() const { return m_name; }
   virtual llvm::StringRef GetDescription(
       llvm::SmallVectorImpl<char>& 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<std::mutex> lock{m_numSinksEnabledMutex};
-    return m_numSinksEnabled;
-  }
+  int GetNumSinksEnabled() const { return m_numSinksEnabled; }
 
   void EnableSink() {
-    std::lock_guard<std::mutex> lock{m_numSinksEnabledMutex};
     ++m_numSinksEnabled;
-    m_numSinksEnabledCv.notify_all();
+    NumSinksEnabledChanged();
   }
 
   void DisableSink() {
-    std::lock_guard<std::mutex> lock{m_numSinksEnabledMutex};
     --m_numSinksEnabled;
-  }
-
-  void WaitForEnabledSink() {
-    std::unique_lock<std::mutex> 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<int> EnumerateProperties(
-      llvm::SmallVectorImpl<int>& vec) const = 0;
+      llvm::SmallVectorImpl<int>& vec, CS_Status* status) const = 0;
   virtual CS_PropertyType GetPropertyType(int property) const = 0;
   virtual llvm::StringRef GetPropertyName(int property,
                                           llvm::SmallVectorImpl<char>& 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<VideoMode> 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<VideoMode> 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<Frame::Data> 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<Frame::Data> 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<std::unique_ptr<Frame::Data>> m_framesAvail;
-  std::vector<Image> m_imagesAvail;
 };
 
 }  // namespace cs

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 <sys/mman.h>
+#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_

src/USBCameraImpl.h

@@ -10,18 +10,20 @@
 
 #include <atomic>
 #include <thread>
+#include <vector>
 
 #ifdef __linux__
 #include <linux/videodev2.h>
 #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<char>& buf) const override;
+  bool IsConnected() const override;
 
   // Property functions
   int GetPropertyIndex(llvm::StringRef name) const override;
-  llvm::ArrayRef<int> EnumerateProperties(
-      llvm::SmallVectorImpl<int>& vec) const override;
+  llvm::ArrayRef<int> EnumerateProperties(llvm::SmallVectorImpl<int>& vec,
+                                          CS_Status* status) const override;
   CS_PropertyType GetPropertyType(int property) const override;
   llvm::StringRef GetPropertyName(int property,
                                   llvm::SmallVectorImpl<char>& 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<VideoMode> 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<VideoMode> 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<std::string> 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<int, PropertyData> m_property_data;
-  mutable llvm::StringMap<int> m_properties;
-  mutable std::atomic_bool m_properties_cached{false};
+  // Message pool access
+  std::unique_ptr<Message> CreateMessage(Message::Type type) const {
+    std::lock_guard<std::mutex> lock(m_mutex);
+    if (m_messagePool.empty()) return llvm::make_unique<Message>();
+    auto rv = std::move(m_messagePool.back());
+    m_messagePool.pop_back();
+    rv->type = type;
+    return rv;
+  }
+  void DestroyMessage(std::unique_ptr<Message> message) const {
+    std::lock_guard<std::mutex> 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<Message> SendAndWait(std::unique_ptr<Message> msg) const;
+  // Send a message to the camera thread with no response
+  void Send(std::unique_ptr<Message> 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<std::mutex>& 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<USBCameraBuffer, kNumBuffers> 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<PropertyData> m_propertyData;
+  mutable llvm::StringMap<int> m_properties;
+  std::vector<VideoMode> 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<size_t>(property) > m_propertyData.size())
+      return nullptr;
+    return &m_propertyData[property - 1];
+  }
+  const PropertyData* GetProperty(int property) const {
+    if (property <= 0 || static_cast<size_t>(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<std::unique_ptr<Message>> m_messagePool;
+  mutable std::vector<std::unique_ptr<Message>> m_commands;
+  mutable std::vector<std::unique_ptr<Message>> m_responses;
+  mutable std::condition_variable m_responseCv;
 
   mutable std::mutex m_mutex;
-  mutable std::condition_variable m_mode_changed;
 };
 
 }  // namespace cs

src/cameraserver_cpp.cpp

@@ -234,7 +234,8 @@ llvm::ArrayRef<CS_Property> EnumerateSourceProperties(
     return 0;
   }
   llvm::SmallVector<int, 32> 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;
 }