From e448b5d069be1bd0d4f7fd6abe41c29f9ddc39ed Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 2 Apr 2018 16:00:36 +1000
Subject: [PATCH] HAL_ChibiOS: implement UART port locking

---
 libraries/AP_HAL_ChibiOS/UARTDriver.cpp | 40 ++++++++++++++++++++++---
 libraries/AP_HAL_ChibiOS/UARTDriver.h   | 11 +++++++
 2 files changed, 47 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
index d14a2a0dcd..af96986710 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
@@ -447,8 +447,8 @@ int16_t UARTDriver::read()
 /* Empty implementations of Print virtual methods */
 size_t UARTDriver::write(uint8_t c)
 {
-    if (!chMtxTryLock(&_write_mutex)) {
-        return -1;
+    if (lock_key != 0 || !chMtxTryLock(&_write_mutex)) {
+        return 0;
     }
     
     if (!_initialised) {
@@ -473,12 +473,12 @@ size_t UARTDriver::write(uint8_t c)
 
 size_t UARTDriver::write(const uint8_t *buffer, size_t size)
 {
-    if (!_initialised) {
+    if (!_initialised || lock_key != 0) {
 		return 0;
 	}
 
     if (!chMtxTryLock(&_write_mutex)) {
-        return -1;
+        return 0;
     }
 
     if (_blocking_writes && !unbuffered_writes) {
@@ -502,6 +502,38 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
     return ret;
 }
 
+/*
+  lock the uart for exclusive use by write_locked() with the right key
+ */
+bool UARTDriver::lock_port(uint32_t key)
+{
+    if (lock_key && key != lock_key && key != 0) {
+        // someone else is using it
+        return false;
+    }
+    lock_key = key;
+    return true;
+}
+
+/* 
+   write to a locked port. If port is locked and key is not correct then 0 is returned
+   and write is discarded. All writes are non-blocking
+*/
+size_t UARTDriver::write_locked(const uint8_t *buffer, size_t size, uint32_t key)
+{
+    if (lock_key != 0 && key != lock_key) {
+        return 0;
+    }
+    if (!chMtxTryLock(&_write_mutex)) {
+        return 0;
+    }
+    size_t ret = _writebuf.write(buffer, size);
+
+    chMtxUnlock(&_write_mutex);
+
+    return ret;
+}
+
 /*
   wait for data to arrive, or a timeout. Return true if data has
   arrived, false on timeout
diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h
index 2659f39215..727e706c88 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.h
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h
@@ -46,6 +46,14 @@ public:
 
     size_t write(uint8_t c);
     size_t write(const uint8_t *buffer, size_t size);
+
+    // lock a port for exclusive use. Use a key of 0 to unlock
+    bool lock_port(uint32_t key) override;
+
+    // write to a locked port. If port is locked and key is not correct then 0 is returned
+    // and write is discarded
+    size_t write_locked(const uint8_t *buffer, size_t size, uint32_t key) override;
+    
     struct SerialDef {
         BaseSequentialStream* serial;
         bool is_usb;
@@ -87,6 +95,9 @@ private:
 
     // index into uart_drivers table
     uint8_t serial_num;
+
+    // key for a locked port
+    uint32_t lock_key;
     
     uint32_t _baudrate;
     uint16_t tx_len;