AP_HAL_Linux: implement methods to open the bus

These are very similar to their counterparts in I2CDriver. The changes
were:
    - Don't use fixed buffer with PATH_MAX length: allocate the string
    - Change the interface to use std::vector so we can simplify the
      implementation

libraries/AP_HAL_Linux/I2CDevice.cpp

@@ -36,6 +36,21 @@ namespace Linux {
 
 static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
 
+/*
+ * TODO: move to Util or other upper class to be used by others
+ *
+ * Return pointer to the next char if @s starts with @prefix, otherwise
+ * returns nullptr.
+ */
+static inline char *startswith(const char *s, const char *prefix)
+{
+    size_t len = strlen(prefix);
+    if (strncmp(s, prefix, len) == 0) {
+        return (char *) s + len;
+    }
+    return nullptr;
+}
+
 /* Private struct to maintain for each bus */
 class I2CBus {
 public:
@@ -55,7 +70,16 @@ public:
             return -EBUSY;
         }
 
-        // TODO: open the bus
+        r = snprintf(path, sizeof(path), "/dev/i2c-%u", n);
+        if (r < 0 || r >= (int)sizeof(path)) {
+            return -EINVAL;
+        }
+
+        fd = ::open(path, O_RDWR | O_CLOEXEC);
+        if (fd < 0) {
+            return -errno;
+        }
+
         bus = n;
 
         return fd;
@@ -100,7 +124,61 @@ I2CDeviceManager::I2CDeviceManager()
 AP_HAL::OwnPtr<AP_HAL::I2CDevice>
 I2CDeviceManager::get_device(std::vector<const char *> devpaths, uint8_t address)
 {
-    // implement device search on sysfs
+    const char *dirname = "/sys/class/i2c-dev";
+    struct dirent *de = nullptr;
+    DIR *d;
+
+    d = opendir(dirname);
+    if (!d) {
+        AP_HAL::panic("Could not get list of I2C buses");
+    }
+
+    for (de = readdir(d); de; de = readdir(d)) {
+        char *str_device, *abs_str_device;
+        const char *p;
+
+        if (strcmp(de->d_name, ".") == 0 || strcmp(de->d_name, "..") == 0) {
+            continue;
+        }
+
+        if (asprintf(&str_device, "%s/%s", dirname, de->d_name) < 0) {
+            continue;
+        }
+
+        abs_str_device = realpath(str_device, nullptr);
+        if (!abs_str_device || !(p = startswith(abs_str_device, "/sys"))) {
+            free(abs_str_device);
+            free(str_device);
+            continue;
+        }
+
+        auto t = std::find_if(std::begin(devpaths), std::end(devpaths),
+                              [p](const char *prefix) {
+                                  return startswith(p, prefix) != nullptr;
+                              });
+
+        free(abs_str_device);
+        free(str_device);
+
+        if (t != std::end(devpaths)) {
+            unsigned int n;
+
+            /* Found the bus, try to create the device now */
+            if (sscanf(de->d_name, "i2c-%u", &n) != 1) {
+                AP_HAL::panic("I2CDevice: can't parse %s", de->d_name);
+            }
+            if (n > UINT8_MAX) {
+                AP_HAL::panic("I2CDevice: bus with number n=%u higher than %u",
+                              n, UINT8_MAX);
+            }
+
+            closedir(d);
+            return get_device(n, address);
+        }
+    }
+
+    /* not found */
+    closedir(d);
     return nullptr;
 }