HAL_ChibiOS: implement UART port locking

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

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;
@@ -88,6 +96,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;
 #if HAL_USE_SERIAL == TRUE