From f1ce321a2f906f43001da97c303098e430a8d756 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 6 Feb 2018 17:48:10 +1100
Subject: [PATCH] HAL_ChibiOS: added checking of bus owner

this ensures all bus transfers are only done by the thread that owns
the semaphore
---
 libraries/AP_HAL_ChibiOS/I2CDevice.cpp | 25 ++++++++++++++-----------
 libraries/AP_HAL_ChibiOS/SPIDevice.cpp |  3 +++
 libraries/AP_HAL_ChibiOS/Semaphores.h  |  6 ++++++
 3 files changed, 23 insertions(+), 11 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
index 3888468019..aa758d2712 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
@@ -126,6 +126,11 @@ void I2CBus::dma_deallocate(void)
 bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
+    if (!bus.semaphore.check_owner()) {
+        hal.console->printf("ERR: I2C transfer by non-owner\n");
+        return false;
+    }
+    
     bus.dma_handle->lock();
 
     if (_use_smbus) {
@@ -172,41 +177,39 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
 
     bus.bouncebuffer_setup(send_buf, send_len, recv_buf, recv_len);
 
+    i2cAcquireBus(I2CD[bus.busnum].i2c);
+    
     for(uint8_t i=0 ; i <= _retries; i++) {
         int ret;
-        i2cAcquireBus(I2CD[bus.busnum].i2c);
         // 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;
+        osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_READY, "i2cStart state");
         if(send_len == 0) {
             ret = i2cMasterReceiveTimeout(I2CD[bus.busnum].i2c, _address, recv_buf, recv_len, MS2ST(timeout_ms));
         } else {
             ret = i2cMasterTransmitTimeout(I2CD[bus.busnum].i2c, _address, send_buf, 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){
+        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_READY || I2CD[bus.busnum].i2c->state == I2C_LOCKED, "i2cStart state");
+            i2cStop(I2CD[bus.busnum].i2c);
             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 {
+            osalDbgAssert(I2CD[bus.busnum].i2c->state == I2C_READY, "i2cStart state");
             if (recv_buf != recv) {
                 memcpy(recv, recv_buf, recv_len);
             }
+            i2cReleaseBus(I2CD[bus.busnum].i2c);
             return true;
         }
     }
+    i2cReleaseBus(I2CD[bus.busnum].i2c);
     return false;
 }
 
diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
index 7f58f6b125..dde40c5ffd 100644
--- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
@@ -196,6 +196,7 @@ uint16_t SPIDevice::derive_freq_flag(uint32_t _frequency)
 bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
+    bus.semaphore.assert_owner();
     if (send_len == recv_len && send == recv) {
         // simplest cases, needed for DMA
         do_transfer(send, recv, recv_len);
@@ -217,6 +218,7 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
 
 bool SPIDevice::transfer_fullduplex(const uint8_t *send, uint8_t *recv, uint32_t len)
 {
+    bus.semaphore.assert_owner();
     uint8_t buf[len];
     memcpy(buf, send, len);
     do_transfer(buf, buf, len);
@@ -245,6 +247,7 @@ bool SPIDevice::adjust_periodic_callback(AP_HAL::Device::PeriodicHandle h, uint3
  */
 bool SPIDevice::set_chip_select(bool set)
 {
+    bus.semaphore.assert_owner();
     if (set && cs_forced) {
         return true;
     }
diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.h b/libraries/AP_HAL_ChibiOS/Semaphores.h
index 15b7150acc..b79d888268 100644
--- a/libraries/AP_HAL_ChibiOS/Semaphores.h
+++ b/libraries/AP_HAL_ChibiOS/Semaphores.h
@@ -29,6 +29,12 @@ public:
     bool give();
     bool take(uint32_t timeout_ms);
     bool take_nonblocking();
+    bool check_owner(void) {
+        return _lock.owner == chThdGetSelfX();
+    }
+    void assert_owner(void) {
+        osalDbgAssert(check_owner(), "owner");
+    }
 private:
     mutex_t _lock;
 };