From 26163b66406cf9b7bf004d8b6830c6f465e7fbb8 Mon Sep 17 00:00:00 2001
From: Julien BERAUD <julien.beraud@parrot.com>
Date: Thu, 26 Nov 2015 14:25:40 +0100
Subject: [PATCH] AP_HAL_Linux: Add do_transfer method to i2c driver

Method needed for mt9v117 camera sensor
---
 libraries/AP_HAL_Linux/I2CDriver.cpp | 43 +++++++++++++++++++++++++++-
 libraries/AP_HAL_Linux/I2CDriver.h   |  2 ++
 2 files changed, 44 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_HAL_Linux/I2CDriver.cpp b/libraries/AP_HAL_Linux/I2CDriver.cpp
index ae1881611e..f9ed52837f 100644
--- a/libraries/AP_HAL_Linux/I2CDriver.cpp
+++ b/libraries/AP_HAL_Linux/I2CDriver.cpp
@@ -308,7 +308,48 @@ uint8_t I2CDriver::readRegister(uint8_t addr, uint8_t reg, uint8_t* data)
     return 0;
 }
 
-uint8_t I2CDriver::lockup_count() 
+/*
+  main transfer function
+ */
+bool I2CDriver::do_transfer(uint8_t addr, const uint8_t *send,
+                            uint32_t send_len, uint8_t *recv,
+                            uint32_t recv_len)
+{
+    struct i2c_msg i2cmsg[2] = {
+        {
+        addr  : addr,
+        flags : 0,
+        len   : (typeof(i2cmsg->len))send_len,
+        buf   : (typeof(i2cmsg->buf))send
+        },
+        {
+        addr  : addr,
+        flags : I2C_M_RD,
+        len   : (typeof(i2cmsg->len))recv_len,
+        buf   : (typeof(i2cmsg->buf))recv,
+        }
+    };
+    struct i2c_rdwr_ioctl_data msg_rdwr;
+
+    if (send_len == 0 && recv_len) {
+        msg_rdwr.msgs = &i2cmsg[1];
+        msg_rdwr.nmsgs = 1;
+    }
+    else if (send_len && recv_len == 0) {
+        msg_rdwr.msgs = &i2cmsg[0];
+        msg_rdwr.nmsgs = 1;
+    }
+    else if (send_len && recv_len) {
+        msg_rdwr.msgs = &i2cmsg[0];
+        msg_rdwr.nmsgs = 2;
+    }
+    else {
+        return false;
+    }
+    return ioctl(_fd, I2C_RDWR, &msg_rdwr) == (int)msg_rdwr.nmsgs;
+}
+
+uint8_t I2CDriver::lockup_count()
 {
     return 0;
 }
diff --git a/libraries/AP_HAL_Linux/I2CDriver.h b/libraries/AP_HAL_Linux/I2CDriver.h
index 861f53fb76..727ee418b3 100644
--- a/libraries/AP_HAL_Linux/I2CDriver.h
+++ b/libraries/AP_HAL_Linux/I2CDriver.h
@@ -41,6 +41,8 @@ public:
     uint8_t lockup_count();
 
     AP_HAL::Semaphore* get_semaphore() { return _semaphore; }
+    bool do_transfer(uint8_t address, const uint8_t *send, uint32_t send_len,
+                     uint8_t *recv, uint32_t recv_len)override;
 
 private:
     bool set_address(uint8_t addr);