From 23e1b2e271286bd02b92f92e2af99201c2468900 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 6 Feb 2018 08:14:55 +1100
Subject: [PATCH] HAL_ChibiOS: added paranoid state checking on I2C

this is here just while debugging an issue with Mark
---
 libraries/AP_HAL_ChibiOS/I2CDevice.cpp | 11 +++++++++++
 libraries/AP_HAL_ChibiOS/I2CDevice.h   |  1 +
 2 files changed, 12 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
index 38ac39f578..3888468019 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
@@ -103,7 +103,9 @@ I2CDevice::~I2CDevice()
 void I2CBus::dma_allocate(void)
 {
     if (!i2c_started) {
+        osalDbgAssert(I2CD[busnum].i2c->state == I2C_STOP, "i2cStart state");
         i2cStart(I2CD[busnum].i2c, &i2ccfg);
+        osalDbgAssert(I2CD[busnum].i2c->state == I2C_READY, "i2cStart state");
         i2c_started = true;
     }
 }
@@ -114,7 +116,9 @@ void I2CBus::dma_allocate(void)
 void I2CBus::dma_deallocate(void)
 {
     if (i2c_started) {
+        osalDbgAssert(I2CD[busnum].i2c->state == I2C_READY, "i2cStart state");
         i2cStop(I2CD[busnum].i2c);
+        osalDbgAssert(I2CD[busnum].i2c->state == I2C_STOP, "i2cStart state");
         i2c_started = false;
     }
 }
@@ -174,6 +178,7 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
         // calculate a timeout as twice the expected transfer time, and set as min of 4ms
         uint32_t timeout_ms = 1+2*(((8*1000000UL/bus.i2ccfg.clock_speed)*MAX(send_len, recv_len))/1000);
         timeout_ms = MAX(timeout_ms, _timeout_ms);
+        bus.i2c_active = true;
         if(send_len == 0) {
             ret = i2cMasterReceiveTimeout(I2CD[bus.busnum].i2c, _address, recv_buf, recv_len, MS2ST(timeout_ms));
         } else {
@@ -181,13 +186,19 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
                                            recv_buf, recv_len, MS2ST(timeout_ms));
         }
         i2cReleaseBus(I2CD[bus.busnum].i2c);
+        bus.i2c_active = false;
+        osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_READY, "i2cStart state");
         if (ret != MSG_OK){
             //restart the bus
             if (bus.i2c_started) {
+                osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_READY, "i2cStart state");
                 i2cStop(I2CD[bus.busnum].i2c);
+                osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_STOP, "i2cStart state");
                 bus.i2c_started = false;
             }
+            osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_STOP, "i2cStart state");
             i2cStart(I2CD[bus.busnum].i2c, &bus.i2ccfg);
+            osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_READY, "i2cStart state");
             bus.i2c_started = true;
         } else {
             if (recv_buf != recv) {
diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h
index 3b1822a2da..b838cd39ba 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.h
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h
@@ -35,6 +35,7 @@ public:
     I2CConfig i2ccfg;
     uint8_t busnum;
     bool i2c_started;
+    bool i2c_active;
     
     void dma_allocate(void);
     void dma_deallocate(void);