From f1e10b0a8f019972c2d27bebca60660570bb7d74 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 6 Aug 2018 16:23:24 +1000
Subject: [PATCH] HAL_Linux: make uart writes thread safe

---
 libraries/AP_HAL_Linux/UARTDriver.cpp | 16 ++++++++++++++--
 libraries/AP_HAL_Linux/UARTDriver.h   |  3 +++
 2 files changed, 17 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp
index 0955f3a0a4..902b636edc 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.cpp
+++ b/libraries/AP_HAL_Linux/UARTDriver.cpp
@@ -296,14 +296,20 @@ size_t UARTDriver::write(uint8_t c)
     if (!_initialised) {
         return 0;
     }
+    if (!_write_mutex.take_nonblocking()) {
+        return 0;
+    }
 
     while (_writebuf.space() == 0) {
         if (_nonblocking_writes) {
+            _write_mutex.give();
             return 0;
         }
         hal.scheduler->delay(1);
     }
-    return _writebuf.write(&c, 1);
+    size_t ret = _writebuf.write(&c, 1);
+    _write_mutex.give();
+    return ret;
 }
 
 /*
@@ -314,6 +320,9 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
     if (!_initialised) {
         return 0;
     }
+    if (!_write_mutex.take_nonblocking()) {
+        return 0;
+    }
     if (!_nonblocking_writes) {
         /*
           use the per-byte delay loop in write() above for blocking writes
@@ -323,10 +332,13 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
             if (write(*buffer++) != 1) break;
             ret++;
         }
+        _write_mutex.give();
         return ret;
     }
 
-    return _writebuf.write(buffer, size);
+    size_t ret = _writebuf.write(buffer, size);
+    _write_mutex.give();
+    return ret;
 }
 
 /*
diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h
index 434075fcc6..956bfbf4dd 100644
--- a/libraries/AP_HAL_Linux/UARTDriver.h
+++ b/libraries/AP_HAL_Linux/UARTDriver.h
@@ -5,6 +5,7 @@
 
 #include "AP_HAL_Linux.h"
 #include "SerialDevice.h"
+#include "Semaphores.h"
 
 namespace Linux {
 
@@ -97,6 +98,8 @@ protected:
 
     virtual int _write_fd(const uint8_t *buf, uint16_t n);
     virtual int _read_fd(uint8_t *buf, uint16_t n);
+
+    Linux::Semaphore _write_mutex;    
 };
 
 }