From 3402d07651608dd173e167597f6b2f146a1f0a28 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 26 Oct 2017 15:08:29 +1100
Subject: [PATCH] HAL_PX4: fixed a race condition on i2c init

we could call init on two devices with the same name, which caused
init_ok to be false. This could cause the SMBus battery to fail to
initialise

Thanks to Michael duBreuil for finding this!
---
 libraries/AP_HAL_PX4/HAL_PX4_Class.cpp | 3 +++
 libraries/AP_HAL_PX4/I2CDevice.cpp     | 6 ++++++
 libraries/AP_HAL_PX4/I2CWrapper.h      | 6 ++++++
 3 files changed, 15 insertions(+)

diff --git a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
index e0fa502dde..f9fc083742 100644
--- a/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
+++ b/libraries/AP_HAL_PX4/HAL_PX4_Class.cpp
@@ -155,6 +155,9 @@ static int main_loop(int argc, char **argv)
     hal.uartE->begin(57600);
     hal.scheduler->init();
 
+    // init the I2C wrapper class
+    PX4_I2C::init_lock();
+    
     /*
       run setup() at low priority to ensure CLI doesn't hang the
       system, and to allow initial sensor read loops to run
diff --git a/libraries/AP_HAL_PX4/I2CDevice.cpp b/libraries/AP_HAL_PX4/I2CDevice.cpp
index 624aa7439b..6a4d9f637e 100644
--- a/libraries/AP_HAL_PX4/I2CDevice.cpp
+++ b/libraries/AP_HAL_PX4/I2CDevice.cpp
@@ -22,6 +22,7 @@
 namespace PX4 {
 
 uint8_t PX4::PX4_I2C::instance;
+pthread_mutex_t PX4::PX4_I2C::instance_lock;
 
 DeviceBus I2CDevice::businfo[I2CDevice::num_buses];
 
@@ -72,14 +73,19 @@ bool PX4_I2C::do_transfer(uint8_t address, const uint8_t *send, uint32_t send_le
 {
     set_address(address);
     if (!init_done) {
+        if (pthread_mutex_lock(&instance_lock) != 0) {
+            return false;
+        }
         init_done = true;
         // we do late init() so we can setup the device paths
+        
         snprintf(devname, sizeof(devname), "AP_I2C_%u", instance);
         snprintf(devpath, sizeof(devpath), "/dev/api2c%u", instance);
         init_ok = (init() == OK);
         if (init_ok) {
             instance++;
         }
+        pthread_mutex_unlock(&instance_lock);
     }
     if (!init_ok) {
         return false;
diff --git a/libraries/AP_HAL_PX4/I2CWrapper.h b/libraries/AP_HAL_PX4/I2CWrapper.h
index b7c64e79c0..f61fa1f053 100644
--- a/libraries/AP_HAL_PX4/I2CWrapper.h
+++ b/libraries/AP_HAL_PX4/I2CWrapper.h
@@ -23,9 +23,15 @@ public:
     }
 
     uint8_t map_bus_number(uint8_t bus) const;
+
+    // setup instance_lock
+    static void init_lock(void) {
+        pthread_mutex_init(&instance_lock, nullptr);
+    }
     
 private:
     static uint8_t instance;
+    static pthread_mutex_t instance_lock;
     bool init_done;
     bool init_ok;
     char devname[10];