From a945c97ec66187316589cbcdd5707b872504a46e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 4 Jun 2018 21:27:54 +1000
Subject: [PATCH] HAL_ChibiOS: fixed 3-way DMA sharing bug

when we have 3 way contention across two DMA streams we could get the
dma_deallocate function called in an object from two places at
once. This adds a mutex that prevents that scenario
---
 libraries/AP_HAL_ChibiOS/I2CDevice.cpp | 5 +++++
 libraries/AP_HAL_ChibiOS/I2CDevice.h   | 6 ++++++
 libraries/AP_HAL_ChibiOS/SPIDevice.cpp | 4 ++++
 libraries/AP_HAL_ChibiOS/SPIDevice.h   | 6 ++++++
 4 files changed, 21 insertions(+)

diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
index bb374c5940..c3c421a014 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.cpp
@@ -53,6 +53,7 @@ I2CBus I2CDeviceManager::businfo[ARRAY_SIZE_SIMPLE(I2CD)];
 // get a handle for DMA sharing DMA channels with other subsystems
 void I2CBus::dma_init(void)
 {
+    chMtxObjectInit(&dma_lock);
     dma_handle = new Shared_DMA(I2CD[busnum].dma_channel_tx, I2CD[busnum].dma_channel_rx, 
                                 FUNCTOR_BIND_MEMBER(&I2CBus::dma_allocate, void, Shared_DMA *),
                                 FUNCTOR_BIND_MEMBER(&I2CBus::dma_deallocate, void, Shared_DMA *));    
@@ -163,12 +164,14 @@ I2CDevice::~I2CDevice()
  */
 void I2CBus::dma_allocate(Shared_DMA *ctx)
 {
+    chMtxLock(&dma_lock);    
     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;
     }
+    chMtxUnlock(&dma_lock);    
 }
 
 /*
@@ -176,12 +179,14 @@ void I2CBus::dma_allocate(Shared_DMA *ctx)
  */
 void I2CBus::dma_deallocate(Shared_DMA *)
 {
+    chMtxLock(&dma_lock);    
     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;
     }
+    chMtxUnlock(&dma_lock);    
 }
 
 bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
diff --git a/libraries/AP_HAL_ChibiOS/I2CDevice.h b/libraries/AP_HAL_ChibiOS/I2CDevice.h
index 39c642d2bf..ece4d06494 100644
--- a/libraries/AP_HAL_ChibiOS/I2CDevice.h
+++ b/libraries/AP_HAL_ChibiOS/I2CDevice.h
@@ -39,6 +39,12 @@ public:
     uint32_t busclock;
     bool i2c_started;
     bool i2c_active;
+
+    // we need an additional lock in the dma_allocate and
+    // dma_deallocate functions to cope with 3-way contention as we
+    // have two DMA channels that we are handling with the shared_dma
+    // code
+    mutex_t dma_lock;
     
     void dma_allocate(Shared_DMA *);
     void dma_deallocate(Shared_DMA *);
diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
index 0fd869b6dd..5e71869be3 100644
--- a/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
+++ b/libraries/AP_HAL_ChibiOS/SPIDevice.cpp
@@ -61,6 +61,8 @@ SPIBus::SPIBus(uint8_t _bus) :
     DeviceBus(APM_SPI_PRIORITY),
     bus(_bus)
 {
+    chMtxObjectInit(&dma_lock);
+    
     // allow for sharing of DMA channels with other peripherals
     dma_handle = new Shared_DMA(spi_devices[bus].dma_channel_rx,
                                 spi_devices[bus].dma_channel_tx,
@@ -82,11 +84,13 @@ void SPIBus::dma_allocate(Shared_DMA *ctx)
  */
 void SPIBus::dma_deallocate(Shared_DMA *ctx)
 {
+    chMtxLock(&dma_lock);    
     // another non-SPI peripheral wants one of our DMA channels
     if (spi_started) {
         spiStop(spi_devices[bus].driver);
         spi_started = false;
     }
+    chMtxUnlock(&dma_lock);    
 }
 
 
diff --git a/libraries/AP_HAL_ChibiOS/SPIDevice.h b/libraries/AP_HAL_ChibiOS/SPIDevice.h
index be94e204ad..6559d5a12d 100644
--- a/libraries/AP_HAL_ChibiOS/SPIDevice.h
+++ b/libraries/AP_HAL_ChibiOS/SPIDevice.h
@@ -34,6 +34,12 @@ public:
     void dma_allocate(Shared_DMA *ctx);
     void dma_deallocate(Shared_DMA *ctx);
     bool spi_started;
+    
+    // we need an additional lock in the dma_allocate and
+    // dma_deallocate functions to cope with 3-way contention as we
+    // have two DMA channels that we are handling with the shared_dma
+    // code
+    mutex_t dma_lock;
 };
 
 struct SPIDesc {