From 91c741ef0784eaef6feb796d4a60078adf913f3c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 2 Mar 2018 21:34:57 +1100
Subject: [PATCH] HAL_ChibiOS: use a non-blocking lock for UART shared DMA

we can have multiple UARTs on the same thread sharing the same DMA TX
channel. That can lead to deadlock with blocking locks on DMA. This
makes UART requests for DMA locks non-blocking to fix the issue
---
 libraries/AP_HAL_ChibiOS/UARTDriver.cpp |  5 +++-
 libraries/AP_HAL_ChibiOS/shared_dma.cpp | 40 ++++++++++++++++++++-----
 libraries/AP_HAL_ChibiOS/shared_dma.h   |  6 ++++
 3 files changed, 43 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
index 7325b5c4cd..d6c96c52cb 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
@@ -546,7 +546,10 @@ void UARTDriver::write_pending_bytes_DMA(uint32_t n)
     if (tx_len == 0) {
         return;
     }
-    dma_handle->lock();
+    if (!dma_handle->lock_nonblock()) {
+        tx_len = 0;
+        return;
+    }
     tx_bounce_buf_ready = false;
     osalDbgAssert(txdma != nullptr, "UART TX DMA allocation failed");
     dmaStreamSetMemory0(txdma, tx_bounce_buf);
diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.cpp b/libraries/AP_HAL_ChibiOS/shared_dma.cpp
index 65aa0c1eb0..9ef0f68447 100644
--- a/libraries/AP_HAL_ChibiOS/shared_dma.cpp
+++ b/libraries/AP_HAL_ChibiOS/shared_dma.cpp
@@ -58,14 +58,8 @@ void Shared_DMA::unregister()
 }
 
 // lock the DMA channels
-void Shared_DMA::lock(void)
+void Shared_DMA::lock_core(void)
 {
-    if (stream_id1 != SHARED_DMA_NONE) {
-        chBSemWait(&locks[stream_id1].semaphore);
-    }
-    if (stream_id2 != SHARED_DMA_NONE) {
-        chBSemWait(&locks[stream_id2].semaphore);
-    }
     // see if another driver has DMA allocated. If so, call their
     // deallocation function
     if (stream_id1 != SHARED_DMA_NONE &&
@@ -94,6 +88,38 @@ void Shared_DMA::lock(void)
     have_lock = true;
 }
 
+// lock the DMA channels, blocking method
+void Shared_DMA::lock(void)
+{
+    if (stream_id1 != SHARED_DMA_NONE) {
+        chBSemWait(&locks[stream_id1].semaphore);
+    }
+    if (stream_id2 != SHARED_DMA_NONE) {
+        chBSemWait(&locks[stream_id2].semaphore);
+    }
+    lock_core();
+}
+
+// lock the DMA channels, non-blocking
+bool Shared_DMA::lock_nonblock(void)
+{
+    if (stream_id1 != SHARED_DMA_NONE) {
+        if (chBSemWaitTimeout(&locks[stream_id1].semaphore, 1) != MSG_OK) {
+            return false;
+        }
+    }
+    if (stream_id2 != SHARED_DMA_NONE) {
+        if (chBSemWaitTimeout(&locks[stream_id2].semaphore, 1) != MSG_OK) {
+            if (stream_id1 != SHARED_DMA_NONE) {
+                chBSemSignal(&locks[stream_id1].semaphore);
+            }
+            return false;
+        }
+    }
+    lock_core();
+    return true;
+}
+
 // unlock the DMA channels
 void Shared_DMA::unlock(void)
 {
diff --git a/libraries/AP_HAL_ChibiOS/shared_dma.h b/libraries/AP_HAL_ChibiOS/shared_dma.h
index c9d73ba9a6..2d5d957cfd 100644
--- a/libraries/AP_HAL_ChibiOS/shared_dma.h
+++ b/libraries/AP_HAL_ChibiOS/shared_dma.h
@@ -41,6 +41,9 @@ public:
     // blocking lock call
     void lock(void);
 
+    // non-blocking lock call
+    bool lock_nonblock(void);
+    
     // unlock call. The DMA channel will not be immediately
     // deallocated. Instead it will be deallocated if another driver
     // needs it
@@ -65,6 +68,9 @@ private:
     uint8_t stream_id2;
     bool have_lock;
 
+    // core of lock call, after semaphores gained
+    void lock_core(void);
+    
     static struct dma_lock {
         // semaphore to ensure only one peripheral uses a DMA channel at a time
         binary_semaphore_t semaphore;