Eliminate write(c) from UARTDrivers

libraries/AP_HAL/Util.cpp

@@ -16,19 +16,17 @@ class BufferPrinter : public AP_HAL::BetterStream {
     BufferPrinter(char* str, size_t size)  :
         _offs(0), _str(str), _size(size)  {}
 
-    size_t write(uint8_t c) override {
-        if (_offs < _size) {
-            _str[_offs] = c;
-        }
-        _offs++;
-        return 1;
-    }
+    // we never fail to write all bytes - we just drop anything that
+    // doesn't fit on the ground.  _offs does move to indicate how
+    // much space would have been required.
     size_t write(const uint8_t *buffer, size_t size) override {
-        size_t n = 0;
-        while (size--) {
-            n += write(*buffer++);
+        if (_offs < _size) {
+            const size_t space_remaining = _size - _offs;
+            const size_t bytes_to_copy = (space_remaining < size) ? space_remaining : size;
+            memcpy(&_str[_offs], buffer, bytes_to_copy);
         }
-        return n;
+        _offs += size;
+        return size;
     }
 
     size_t _offs;

libraries/AP_HAL/utility/BetterStream.h

@@ -33,7 +33,7 @@ class AP_HAL::BetterStream {
     void print(const char *str) { write(str); }
     void println(const char *str) { printf("%s\r\n", str); }
 
-    virtual size_t write(uint8_t) = 0;
+    size_t write(uint8_t c) { return write(&c, 1); }
     virtual size_t write(const uint8_t *buffer, size_t size) = 0;
     size_t write(const char *str);
 

libraries/AP_HAL/utility/print_vprintf.cpp

@@ -378,9 +378,12 @@ void print_vprintf(AP_HAL::BetterStream *s, const char *fmt, va_list ap)
                 }
 
                 while (size) {
-                    s->write(*pnt++);
-                    if (width) width -= 1;
-                    size -= 1;
+                    const uint32_t written = s->write((uint8_t*)pnt, size);
+                    if (written == 0) {
+                        break;
+                    }
+                    width -= (written < width) ? written : width;
+                    size -= written;
                 }
                 goto tail;
             }

libraries/AP_HAL_ChibiOS/UARTDriver.cpp

@@ -573,63 +573,34 @@ int16_t UARTDriver::read_locked(uint32_t key)
     return byte;
 }
 
-/* Empty implementations of Print virtual methods */
-size_t UARTDriver::write(uint8_t c)
-{
-    if (lock_write_key != 0 || !_write_mutex.take_nonblocking()) {
-        return 0;
-    }
-
-    if (!_initialised) {
-        _write_mutex.give();
-        return 0;
-    }
-
-    while (_writebuf.space() == 0) {
-        if (!_blocking_writes || unbuffered_writes) {
-            _write_mutex.give();
-            return 0;
-        }
-        hal.scheduler->delay(1);
-    }
-    size_t ret = _writebuf.write(&c, 1);
-    if (unbuffered_writes) {
-        write_pending_bytes();
-    }
-    _write_mutex.give();
-    return ret;
-}
-
 size_t UARTDriver::write(const uint8_t *buffer, size_t size)
 {
     if (!_initialised || lock_write_key != 0) {
 		return 0;
 	}
 
-    if (_blocking_writes && unbuffered_writes) {
+    if (_blocking_writes) {
         _write_mutex.take_blocking();
     } else {
         if (!_write_mutex.take_nonblocking()) {
             return 0;
         }
     }
-
-    if (_blocking_writes && !unbuffered_writes) {
-        /*
-          use the per-byte delay loop in write() above for blocking writes
-         */
-        _write_mutex.give();
-        size_t ret = 0;
-        while (size--) {
-            if (write(*buffer++) != 1) break;
-            ret++;
+    size_t ret = 0;
+    while (ret < size) {
+        const uint32_t written = _writebuf.write(buffer, size - ret);
+        if (written == 0) {
+            if (!_blocking_writes) {
+                break;
+            }
+            hal.scheduler->delay(1);
+            continue;
+        }
+        ret += written;
+        buffer += written;
+        if (unbuffered_writes) {
+            write_pending_bytes();
         }
-        return ret;
-    }
-
-    size_t ret = _writebuf.write(buffer, size);
-    if (unbuffered_writes) {
-        write_pending_bytes();
     }
     _write_mutex.give();
     return ret;

libraries/AP_HAL_ChibiOS/UARTDriver.h

@@ -47,7 +47,6 @@ class ChibiOS::UARTDriver : public AP_HAL::UARTDriver {
     int16_t read_locked(uint32_t key) override;
     void _timer_tick(void) override;
 
-    size_t write(uint8_t c) override;
     size_t write(const uint8_t *buffer, size_t size) override;
 
     // lock a port for exclusive use. Use a key of 0 to unlock

libraries/AP_HAL_Empty/UARTDriver.cpp

@@ -17,13 +17,7 @@ uint32_t Empty::UARTDriver::txspace() { return 1; }
 int16_t Empty::UARTDriver::read() { return -1; }
 
 /* Empty implementations of Print virtual methods */
-size_t Empty::UARTDriver::write(uint8_t c) { return 0; }
-
 size_t Empty::UARTDriver::write(const uint8_t *buffer, size_t size)
 {
-    size_t n = 0;
-    while (size--) {
-        n += write(*buffer++);
-    }
-    return n;
+    return 0;
 }

libraries/AP_HAL_Empty/UARTDriver.h

@@ -20,6 +20,5 @@ class Empty::UARTDriver : public AP_HAL::UARTDriver {
     int16_t read() override;
 
     /* Empty implementations of Print virtual methods */
-    size_t write(uint8_t c) override;
     size_t write(const uint8_t *buffer, size_t size) override;
 };

libraries/AP_HAL_Linux/UARTDriver.cpp

@@ -243,7 +243,7 @@ bool UARTDriver::is_initialized()
  */
 void UARTDriver::set_blocking_writes(bool blocking)
 {
-    _nonblocking_writes = !blocking;
+    _blocking_writes = blocking;
 }
 
 
@@ -292,27 +292,6 @@ int16_t UARTDriver::read()
 }
 
 /* Linux implementations of Print virtual methods */
-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);
-    }
-    size_t ret = _writebuf.write(&c, 1);
-    _write_mutex.give();
-    return ret;
-}
-
 /*
   write size bytes to the write buffer
  */
@@ -321,23 +300,27 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size)
     if (!_initialised) {
         return 0;
     }
-    if (!_write_mutex.take_nonblocking()) {
-        return 0;
+
+    if (_blocking_writes) {
+        _write_mutex.take_blocking();
+    } else {
+        if (!_write_mutex.take_nonblocking()) {
+            return 0;
+        }
     }
-    if (!_nonblocking_writes) {
-        /*
-          use the per-byte delay loop in write() above for blocking writes
-         */
-        _write_mutex.give();
-        size_t ret = 0;
-        while (size--) {
-            if (write(*buffer++) != 1) break;
-            ret++;
+    size_t ret = 0;
+    while (ret < size) {
+        const uint32_t written = _writebuf.write(buffer, size - ret);
+        if (written == 0) {
+            if (!_blocking_writes) {
+                break;
+            }
+            hal.scheduler->delay(1);
+            continue;
         }
-        return ret;
+        ret += written;
+        buffer += written;
     }
-
-    size_t ret = _writebuf.write(buffer, size);
     _write_mutex.give();
     return ret;
 }

libraries/AP_HAL_Linux/UARTDriver.h

@@ -32,7 +32,6 @@ class UARTDriver : public AP_HAL::UARTDriver {
     int16_t read() override;
 
     /* Linux implementations of Print virtual methods */
-    size_t write(uint8_t c) override;
     size_t write(const uint8_t *buffer, size_t size) override;
 
     void set_device_path(const char *path);
@@ -69,7 +68,7 @@ class UARTDriver : public AP_HAL::UARTDriver {
 
 private:
     AP_HAL::OwnPtr<SerialDevice> _device;
-    bool _nonblocking_writes;
+    bool _blocking_writes = true;
     bool _console;
     volatile bool _in_timer;
     uint16_t _base_port;

libraries/AP_HAL_SITL/UARTDriver.cpp

@@ -178,19 +178,6 @@ void UARTDriver::flush(void)
 {
 }
 
-// size_t UARTDriver::write(uint8_t c)
-// {
-//     if (txspace() <= 0) {
-//         return 0;
-//     }
-//     _writebuffer.write(&c, 1);
-//     return 1;
-// }
-
-size_t UARTDriver::write(uint8_t c)
-{
-    return write(&c, 1);
-}
 size_t UARTDriver::write(const uint8_t *buffer, size_t size)
 {
     if (txspace() <= size) {

libraries/AP_HAL_SITL/UARTDriver.h

@@ -35,7 +35,7 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
 
     void set_blocking_writes(bool blocking) override
     {
-        _nonblocking_writes = !blocking;
+        _blocking_writes = blocking;
     }
 
     bool tx_pending() override {
@@ -48,7 +48,6 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
     int16_t read() override;
 
     /* Implementations of Print virtual methods */
-    size_t write(uint8_t c) override;
     size_t write(const uint8_t *buffer, size_t size) override;
 
     // file descriptor, exposed so SITL_State::loop_hook() can use it
@@ -89,7 +88,7 @@ class HALSITL::UARTDriver : public AP_HAL::UARTDriver {
     int _listen_fd;  // socket we are listening on
     int _serial_port;
     static bool _console;
-    bool _nonblocking_writes;
+    bool _blocking_writes = true;
     ByteBuffer _readbuffer{16384};
     ByteBuffer _writebuffer{16384};