AP_HAL: moved locking to AP_HAL and added monitor callback

libraries/AP_HAL/UARTDriver.cpp

@@ -0,0 +1,154 @@
+/*
+  implement generic UARTDriver code, including port locking
+ */
+#include "AP_HAL.h"
+
+void AP_HAL::UARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
+{
+    if (lock_write_key != 0) {
+        // silently fail
+        return;
+    }
+    return _begin(baud, rxSpace, txSpace);
+}
+
+void AP_HAL::UARTDriver::begin(uint32_t baud)
+{
+    return begin(baud, 0, 0);
+}
+
+/*
+  lock the uart for exclusive use by write_locked() and read_locked() with the right key
+ */
+bool AP_HAL::UARTDriver::lock_port(uint32_t write_key, uint32_t read_key)
+{
+    if (lock_write_key != 0 && write_key != lock_write_key && write_key != 0) {
+        // someone else is using it
+        return false;
+    }
+    if (lock_read_key != 0 && read_key != lock_read_key && read_key != 0) {
+        // someone else is using it
+        return false;
+    }
+    lock_write_key = write_key;
+    lock_read_key = read_key;
+    return true;
+}
+
+void AP_HAL::UARTDriver::begin_locked(uint32_t baud, uint16_t rxSpace, uint16_t txSpace, uint32_t key)
+{
+    if (lock_write_key != 0 && key != lock_write_key) {
+        // silently fail
+        return;
+    }
+    return _begin(baud, rxSpace, txSpace);
+}
+
+/*
+   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 AP_HAL::UARTDriver::write_locked(const uint8_t *buffer, size_t size, uint32_t key)
+{
+    if (lock_write_key != 0 && key != lock_write_key) {
+        return 0;
+    }
+    return _write(buffer, size);
+}
+
+/*
+   read from a locked port. If port is locked and key is not correct then -1 is returned
+*/
+ssize_t AP_HAL::UARTDriver::read_locked(uint8_t *buf, size_t count, uint32_t key)
+{
+    if (lock_read_key != 0 && key != lock_read_key) {
+        return 0;
+    }
+    ssize_t ret = _read(buf, count);
+#if AP_UART_MONITOR_ENABLED
+    auto monitor = _monitor_read_buffer;
+    if (monitor != nullptr && ret > 0) {
+        monitor->write(buf, ret);
+    }
+#endif
+    return ret;
+}
+
+uint32_t AP_HAL::UARTDriver::available_locked(uint32_t key)
+{
+    if (lock_read_key != 0 && lock_read_key != key) {
+        return 0;
+    }
+    return _available();
+}
+
+size_t AP_HAL::UARTDriver::write(const uint8_t *buffer, size_t size)
+{
+    if (lock_write_key != 0) {
+        return 0;
+    }
+    return _write(buffer, size);
+}
+
+size_t AP_HAL::UARTDriver::write(uint8_t c)
+{
+    return write(&c, 1);
+}
+
+size_t AP_HAL::UARTDriver::write(const char *str)
+{
+    return write((const uint8_t *)str, strlen(str));
+}
+
+ssize_t AP_HAL::UARTDriver::read(uint8_t *buffer, uint16_t count)
+{
+    return read_locked(buffer, count, 0);
+}
+
+bool AP_HAL::UARTDriver::read(uint8_t &b)
+{
+    ssize_t n = read(&b, 1);
+    return n > 0;
+}
+
+int16_t AP_HAL::UARTDriver::read(void)
+{
+    uint8_t b;
+    if (!read(b)) {
+        return -1;
+    }
+    return b;
+}
+
+
+uint32_t AP_HAL::UARTDriver::available()
+{
+    if (lock_read_key != 0) {
+        return 0;
+    }
+    return _available();
+}
+
+void AP_HAL::UARTDriver::end()
+{
+    if (lock_read_key != 0 || lock_write_key != 0) {
+        return;
+    }
+    _end();
+}
+
+void AP_HAL::UARTDriver::flush()
+{
+    if (lock_read_key != 0 || lock_write_key != 0) {
+        return;
+    }
+    _flush();
+}
+
+bool AP_HAL::UARTDriver::discard_input()
+{
+    if (lock_read_key != 0) {
+        return false;
+    }
+    return _discard_input();
+}

libraries/AP_HAL/UARTDriver.h

@@ -7,66 +7,76 @@
 
 #ifndef HAL_UART_STATS_ENABLED
 #define HAL_UART_STATS_ENABLED !defined(HAL_NO_UARTDRIVER)
+#endif
 
+#ifndef AP_UART_MONITOR_ENABLED
+#define AP_UART_MONITOR_ENABLED 0
 #endif
 
 class ExpandingString;
+class ByteBuffer;
 
 /* Pure virtual UARTDriver class */
 class AP_HAL::UARTDriver : public AP_HAL::BetterStream {
-private:
-    // option bits for port
-    uint16_t _last_options;
-
 public:
     UARTDriver() {}
     /* Do not allow copies */
     CLASS_NO_COPY(UARTDriver);
 
-    // begin() implicitly clears rx/tx buffers, even if the port was already open (unless the UART is the console UART)
-    virtual void begin(uint32_t baud) = 0;
-    virtual void begin_locked(uint32_t baud, uint32_t key) { begin(baud); }
+    /*
+      the individual HALs need to implement the protected versions of these calls, _begin(), _write(), _read()
+      port locking is checked by the top level AP_HAL functions
+     */
+    void begin(uint32_t baud);
+
+    /*
+      begin with specified minimum tx and rx buffer sizes
+     */
+    void begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace);
+
+    /*
+      begin for use when the port is write locked. Note that this does
+      not lock the port, an existing write_key from lock_port() must
+      be used
+     */
+    void begin_locked(uint32_t baud, uint16_t rxSpace, uint16_t txSpace, uint32_t write_key);
+
+    /*
+      single and multi-byte write methods
+     */
+    size_t write(uint8_t c) override;
+    size_t write(const uint8_t *buffer, size_t size) override;
+    size_t write(const char *str) override;
+
+    /*
+      single and multi-byte read methods
+     */
+    int16_t read(void) override;
+    bool read(uint8_t &b) override WARN_IF_UNUSED;
+    ssize_t read(uint8_t *buffer, uint16_t count) override;
+    
+    void end();
+    void flush();
 
-	/// Extended port open method
-	///
-	/// Allows for both opening with specified buffer sizes, and re-opening
-	/// to adjust a subset of the port's settings.
-	///
-	/// @note	Buffer sizes greater than ::_max_buffer_size will be rounded
-	///			down.
-	///
-	/// @param	baud		Selects the speed that the port will be
-	///						configured to.  If zero, the port speed is left
-	///						unchanged.
-	/// @param rxSpace		Sets the receive buffer size for the port.  If zero
-	///						then the buffer size is left unchanged if the port
-	///						is open, or set to ::_default_rx_buffer_size if it is
-	///						currently closed.
-	/// @param txSpace		Sets the transmit buffer size for the port.  If zero
-	///						then the buffer size is left unchanged if the port
-	///						is open, or set to ::_default_tx_buffer_size if it
-	///						is currently closed.
-	///
-    virtual void begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) = 0;
-    virtual void end() = 0;
-    virtual void flush() = 0;
     virtual bool is_initialized() = 0;
-    virtual void set_blocking_writes(bool blocking) = 0;
     virtual bool tx_pending() = 0;
 
     // lock a port for exclusive use. Use a key of 0 to unlock
-    virtual bool lock_port(uint32_t write_key, uint32_t read_key) { return false; }
+    bool lock_port(uint32_t write_key, uint32_t read_key);
 
-    // check data available on a locked port. If port is locked and key is not correct then 
-    // 0 is returned
-    virtual uint32_t available_locked(uint32_t key) { return 0; }
+    // check data available for read, return 0 is not available
+    uint32_t available() override;
+    uint32_t available_locked(uint32_t key);
+
+    // discard any pending input
+    bool discard_input() override;
 
     // write to a locked port. If port is locked and key is not correct then 0 is returned
     // and write is discarded
-    virtual size_t write_locked(const uint8_t *buffer, size_t size, uint32_t key) { return 0; }
+    size_t write_locked(const uint8_t *buffer, size_t size, uint32_t key);
 
-    // read from a locked port. If port is locked and key is not correct then 0 is returned
-    virtual bool read_locked(uint32_t key, uint8_t &b) WARN_IF_UNUSED { return -1; }
+    // read buffer from a locked port. If port is locked and key is not correct then -1 is returned
+    ssize_t read_locked(uint8_t *buf, size_t count, uint32_t key) WARN_IF_UNUSED;
     
     // control optional features
     virtual bool set_options(uint16_t options) { _last_options = options; return options==0; }
@@ -160,4 +170,57 @@ public:
 
     // disable TX/RX pins for unusued uart
     virtual void disable_rxtx(void) const {}
+
+#if AP_UART_MONITOR_ENABLED
+    // a way to monitor all reads from the UART, putting them in a
+    // buffer. Used by AP_Periph for GPS debug
+    bool set_monitor_read_buffer(ByteBuffer *buffer) {
+        _monitor_read_buffer = buffer;
+        return true;
+    }
+#endif
+
+protected:
+    // key for a locked port
+    uint32_t lock_write_key;
+    uint32_t lock_read_key;
+
+    /*
+      backend begin method
+     */
+    virtual void _begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) = 0;
+
+    /*
+      backend write method
+     */
+    virtual size_t _write(const uint8_t *buffer, size_t size) = 0;
+
+    /*
+      backend read method
+     */
+    virtual ssize_t _read(uint8_t *buffer, uint16_t count)  WARN_IF_UNUSED = 0;
+
+    /*
+      end control of the port, freeing buffers
+     */
+    virtual void _end() = 0;
+
+    /*
+      flush any pending data
+     */
+    virtual void _flush() = 0;
+
+    // check available data on the port
+    virtual uint32_t _available() = 0;
+
+    // discard incoming data on the port
+    virtual bool _discard_input(void) = 0;
+    
+private:
+    // option bits for port
+    uint16_t _last_options;
+
+#if AP_UART_MONITOR_ENABLED
+    ByteBuffer *_monitor_read_buffer;
+#endif
 };

libraries/AP_HAL/board/sitl.h

@@ -78,3 +78,7 @@
 #ifndef HAL_CAN_DRIVER_DEFAULT
 #define HAL_CAN_DRIVER_DEFAULT 0
 #endif
+
+#ifndef AP_UART_MONITOR_ENABLED
+#define AP_UART_MONITOR_ENABLED 1
+#endif

libraries/AP_HAL/utility/BetterStream.cpp

@@ -29,8 +29,9 @@ int16_t AP_HAL::BetterStream::read()
     return b;
 }
 
-ssize_t AP_HAL::BetterStream::read(uint8_t *buffer, uint16_t count) {
-    uint16_t offset = 0;
+ssize_t AP_HAL::BetterStream::read(uint8_t *buffer, uint16_t count)
+{
+    size_t offset = 0;
     while (count--) {
         const int16_t x = read();
         if (x == -1) {

libraries/AP_HAL/utility/BetterStream.h

@@ -36,11 +36,11 @@ public:
 
     virtual size_t write(uint8_t) = 0;
     virtual size_t write(const uint8_t *buffer, size_t size) = 0;
-    size_t write(const char *str);
+    virtual size_t write(const char *str);
 
     virtual uint32_t available() = 0;
 
-    int16_t read();  // old function; prefer calling the boolean method
+    virtual int16_t read(void);
     virtual bool read(uint8_t &b) WARN_IF_UNUSED = 0;
 
     // no base-class implementation to force descendants to