1 files changed, 42 insertions, 22 deletions

diff --git a/mpm/lib/i2c/i2cdev_iface.cpp b/mpm/lib/i2c/i2cdev_iface.cpp
index d47d0f788..5b59e06f8 100644
--- a/mpm/lib/i2c/i2cdev_iface.cpp
+++ b/mpm/lib/i2c/i2cdev_iface.cpp
@@ -30,39 +30,38 @@ public:
             const std::string &device,
             const uint16_t addr,
             const bool ten_bit_addr,
-            const unsigned int timeout_ms
-    ) : _addr(addr),
+            const unsigned int timeout_ms,
+            const bool do_open = false
+    ) : _device(device),
+        _addr(addr),
         _ten_bit_addr(ten_bit_addr),
         _timeout_ms(timeout_ms)
     {
-        if (i2cdev_open(
-                &_fd,
-                device.c_str(),
-                timeout_ms) < 0)
-        {
-            throw mpm::runtime_error(str(
-                boost::format("Could not initialize i2cdev device %s")
-                % device));
-        }
-
-        if (_fd < 0)
-        {
-            throw mpm::runtime_error(str(
-                boost::format("Could not open i2cdev device %s")
-                % device));
-        }
+        if (do_open)
+            _open();
+        else
+            _fd = -ENODEV;
     }
 
     ~i2cdev_iface_impl()
     {
-        close(_fd);
+        if (_fd >= 0)
+            close(_fd);
     }
 
-    int transfer(uint8_t *tx, size_t tx_len, uint8_t *rx, size_t rx_len)
+    int transfer(uint8_t *tx, size_t tx_len, uint8_t *rx, size_t rx_len, bool do_close)
     {
+        if (_fd < 0)
+            _open();
+
         int ret = i2cdev_transfer(_fd, _addr, _ten_bit_addr,
                                   tx, tx_len, rx, rx_len);
 
+        if (do_close) {
+            close(_fd);
+            _fd = -ENODEV;
+        }
+
         if (ret) {
             throw mpm::runtime_error(str(
                     boost::format("I2C Transaction failed!")
@@ -72,7 +71,7 @@ public:
         return ret;
     }
 
-    int transfer(std::vector<uint8_t> *tx, std::vector<uint8_t> *rx)
+    int transfer(std::vector<uint8_t> *tx, std::vector<uint8_t> *rx, bool do_close)
     {
         uint8_t *tx_data = NULL, *rx_data = NULL;
         size_t tx_len = 0, rx_len = 0;
@@ -86,15 +85,36 @@ public:
             rx_data = rx->data();
             rx_len = rx->size();
         }
-        int ret = transfer(tx_data, tx_len, rx_data, rx_len);
+        int ret = transfer(tx_data, tx_len, rx_data, rx_len, do_close);
+
         return ret;
     }
 
 private:
+    const std::string _device;
     int _fd;
     const uint16_t _addr;
     const bool _ten_bit_addr;
     const unsigned int _timeout_ms;
+
+    int _open(void) {
+         if (i2cdev_open(
+                &_fd,
+                _device.c_str(),
+                _timeout_ms) < 0)
+        {
+            throw mpm::runtime_error(str(
+                boost::format("Could not initialize i2cdev device %s")
+                % _device));
+        }
+
+        if (_fd < 0)
+        {
+            throw mpm::runtime_error(str(
+                boost::format("Could not open i2cdev device %s")
+                % _device));
+        }
+    }
 };
 
 /******************************************************************************