CameraServer: Add VID/PID support for Linux USB devices (#1960)

cscore/src/main/java/edu/wpi/cscore/UsbCameraInfo.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
+/* Copyright (c) 2016-2019 FIRST. 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.                                                               */
@@ -18,13 +18,18 @@ public class UsbCameraInfo {
    * @param path Path to device if available (e.g. '/dev/video0' on Linux)
    * @param name Vendor/model name of the camera as provided by the USB driver
    * @param otherPaths Other path aliases to device
+   * @param vendorId USB vendor id
+   * @param productId USB product id
    */
   @SuppressWarnings("PMD.ArrayIsStoredDirectly")
-  public UsbCameraInfo(int dev, String path, String name, String[] otherPaths) {
+  public UsbCameraInfo(int dev, String path, String name, String[] otherPaths, int vendorId,
+      int productId) {
     this.dev = dev;
     this.path = path;
     this.name = name;
     this.otherPaths = otherPaths;
+    this.vendorId = vendorId;
+    this.productId = productId;
   }
 
   /**
@@ -50,4 +55,16 @@ public class UsbCameraInfo {
    */
   @SuppressWarnings("MemberName")
   public String[] otherPaths;
+
+  /**
+   * USB vendor id.
+   */
+  @SuppressWarnings("MemberName")
+  public int vendorId;
+
+  /**
+   * USB product id.
+   */
+  @SuppressWarnings("MemberName")
+  public int productId;
 }

cscore/src/main/native/cpp/UsbCameraImplCommon.cpp

@@ -21,6 +21,8 @@ static void ConvertToC(CS_UsbCameraInfo* out, const UsbCameraInfo& in) {
   out->otherPathsCount = in.otherPaths.size();
   for (size_t i = 0; i < in.otherPaths.size(); ++i)
     out->otherPaths[i] = cs::ConvertToC(in.otherPaths[i]);
+  out->vendorId = in.vendorId;
+  out->productId = in.productId;
 }
 
 static void FreeUsbCameraInfo(CS_UsbCameraInfo* info) {

cscore/src/main/native/cpp/jni/CameraServerJNI.cpp

@@ -194,13 +194,14 @@ static inline bool CheckStatus(JNIEnv* env, CS_Status status) {
 static jobject MakeJObject(JNIEnv* env, const cs::UsbCameraInfo& info) {
   static jmethodID constructor = env->GetMethodID(
       usbCameraInfoCls, "<init>",
-      "(ILjava/lang/String;Ljava/lang/String;[Ljava/lang/String;)V");
+      "(ILjava/lang/String;Ljava/lang/String;[Ljava/lang/String;II)V");
   JLocal<jstring> path(env, MakeJString(env, info.path));
   JLocal<jstring> name(env, MakeJString(env, info.name));
   JLocal<jobjectArray> otherPaths(env, MakeJStringArray(env, info.otherPaths));
   return env->NewObject(usbCameraInfoCls, constructor,
                         static_cast<jint>(info.dev), path.obj(), name.obj(),
-                        otherPaths.obj());
+                        otherPaths.obj(), static_cast<jint>(info.vendorId),
+                        static_cast<jint>(info.productId));
 }
 
 static jobject MakeJObject(JNIEnv* env, const cs::VideoMode& videoMode) {

cscore/src/main/native/include/cscore_c.h

@@ -236,6 +236,8 @@ typedef struct CS_UsbCameraInfo {
   char* name;
   int otherPathsCount;
   char** otherPaths;
+  int vendorId;
+  int productId;
 } CS_UsbCameraInfo;
 
 /**

cscore/src/main/native/include/cscore_cpp.h

@@ -56,6 +56,10 @@ struct UsbCameraInfo {
   std::string name;
   /** Other path aliases to device (e.g. '/dev/v4l/by-id/...' etc on Linux) */
   std::vector<std::string> otherPaths;
+  /** USB Vendor Id */
+  int vendorId = -1;
+  /** USB Product Id */
+  int productId = -1;
 };
 
 /**

cscore/src/main/native/linux/UsbCameraImpl.cpp

@@ -143,10 +143,36 @@ int UsbCameraImpl::PercentageToRaw(const UsbCameraProperty& rawProp,
          (rawProp.maximum - rawProp.minimum) * (percentValue / 100.0);
 }
 
-static bool GetDescriptionSysV4L(wpi::StringRef path, std::string* desc) {
-  wpi::SmallString<64> ifpath{"/sys/class/video4linux/"};
-  ifpath += path.substr(5);
-  ifpath += "/device/interface";
+static bool GetVendorProduct(int dev, int* vendor, int* product) {
+  wpi::SmallString<64> ifpath;
+  {
+    wpi::raw_svector_ostream oss{ifpath};
+    oss << "/sys/class/video4linux/video" << dev << "/device/modalias";
+  }
+
+  int fd = open(ifpath.c_str(), O_RDONLY);
+  if (fd < 0) return false;
+
+  char readBuf[128];
+  ssize_t n = read(fd, readBuf, sizeof(readBuf));
+  close(fd);
+
+  if (n <= 0) return false;
+  wpi::StringRef readStr{readBuf};
+  if (readStr.substr(readStr.find('v')).substr(1, 4).getAsInteger(16, *vendor))
+    return false;
+  if (readStr.substr(readStr.find('p')).substr(1, 4).getAsInteger(16, *product))
+    return false;
+
+  return true;
+}
+
+static bool GetDescriptionSysV4L(int dev, std::string* desc) {
+  wpi::SmallString<64> ifpath;
+  {
+    wpi::raw_svector_ostream oss{ifpath};
+    oss << "/sys/class/video4linux/video" << dev << "/device/interface";
+  }
 
   int fd = open(ifpath.c_str(), O_RDONLY);
   if (fd < 0) return false;
@@ -192,23 +218,35 @@ static bool GetDescriptionIoctl(const char* cpath, std::string* desc) {
   return true;
 }
 
-static std::string GetDescriptionImpl(const char* cpath) {
+static int GetDeviceNum(const char* cpath) {
   wpi::StringRef path{cpath};
-  char pathBuf[128];
-  std::string rv;
+  std::string pathBuf;
 
-  // If trying to get by id or path, follow symlink
-  if (path.startswith("/dev/v4l/by-id/")) {
-    ssize_t n = readlink(cpath, pathBuf, sizeof(pathBuf));
-    if (n > 0) path = wpi::StringRef(pathBuf, n);
-  } else if (path.startswith("/dev/v4l/by-path/")) {
-    ssize_t n = readlink(cpath, pathBuf, sizeof(pathBuf));
-    if (n > 0) path = wpi::StringRef(pathBuf, n);
+  // it might be a symlink; if so, find the symlink target (e.g. /dev/videoN),
+  // add that to the list and make it the keypath
+  if (wpi::sys::fs::is_symlink_file(cpath)) {
+    char* target = ::realpath(cpath, nullptr);
+    if (target) {
+      pathBuf = target;
+      path = pathBuf;
+      std::free(target);
+    }
   }
 
-  if (path.startswith("/dev/video")) {
+  path = wpi::sys::path::filename(path);
+  if (!path.startswith("video")) return -1;
+  int dev = -1;
+  if (path.substr(5).getAsInteger(10, dev)) return -1;
+  return dev;
+}
+
+static std::string GetDescriptionImpl(const char* cpath) {
+  std::string rv;
+
+  int dev = GetDeviceNum(cpath);
+  if (dev >= 0) {
     // Sometimes the /sys tree gives a better name.
-    if (GetDescriptionSysV4L(path, &rv)) return rv;
+    if (GetDescriptionSysV4L(dev, &rv)) return rv;
   }
 
   // Otherwise use an ioctl to query the caps and get the card name
@@ -1318,24 +1356,15 @@ UsbCameraInfo GetUsbCameraInfo(CS_Source source, CS_Status* status) {
   std::string keypath = static_cast<UsbCameraImpl&>(*data->source).GetPath();
   info.path = keypath;
 
-  // it might be a symlink; if so, find the symlink target (e.g. /dev/videoN),
-  // add that to the list and make it the keypath
-  if (wpi::sys::fs::is_symlink_file(keypath)) {
-    char* target = ::realpath(keypath.c_str(), nullptr);
-    if (target) {
-      keypath.assign(target);
-      info.otherPaths.emplace_back(keypath);
-      std::free(target);
-    }
-  }
-
   // device number
-  wpi::StringRef fname = wpi::sys::path::filename(keypath);
-  if (fname.startswith("video")) fname.substr(5).getAsInteger(10, info.dev);
+  info.dev = GetDeviceNum(keypath.c_str());
 
   // description
   info.name = GetDescriptionImpl(keypath.c_str());
 
+  // vendor/product id
+  GetVendorProduct(info.dev, &info.vendorId, &info.productId);
+
   // look through /dev/v4l/by-id and /dev/v4l/by-path for symlinks to the
   // keypath
   wpi::SmallString<128> path;
@@ -1386,6 +1415,8 @@ std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status) {
       info.name = GetDescriptionImpl(path.c_str());
       if (info.name.empty()) continue;
 
+      GetVendorProduct(dev, &info.vendorId, &info.productId);
+
       if (dev >= retval.size()) retval.resize(info.dev + 1);
       retval[info.dev] = std::move(info);
     }