From c7a65a026cd51d51fb6656c51bb3bc6981e61674 Mon Sep 17 00:00:00 2001 From: Murilo Belluzzo Date: Sat, 30 Jul 2016 08:54:37 -0300 Subject: [PATCH] AP_HAL_PX4: UARTDriver: Make use of ByteBuffer class This patch replaces the 'old style' ringbuffer by the ByteBuffer class. An effort was made to keep the exchange as close as possible from a drop-in replacement to minimize the risk of introducing bugs. Although the exchange opens opportunities for improvement and simplification of this class. --- libraries/AP_HAL_Linux/UARTDriver.cpp | 14 +- libraries/AP_HAL_PX4/UARTDriver.cpp | 181 ++++++++------------------ libraries/AP_HAL_PX4/UARTDriver.h | 16 +-- 3 files changed, 65 insertions(+), 146 deletions(-) diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index 8d0e8f42c0..c03061d478 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -355,14 +355,12 @@ int UARTDriver::_read_fd(uint8_t *buf, uint16_t n) */ bool UARTDriver::_write_pending_bytes(void) { - uint16_t n; - // write any pending bytes uint32_t available_bytes = _writebuf.available(); - n = available_bytes; + uint16_t n = available_bytes; + int16_t b = _writebuf.peek(0); if (_packetise && n > 0 && - (_writebuf.peek(0) != MAVLINK_STX_MAVLINK1 && - _writebuf.peek(0) != MAVLINK_STX)) { + b != MAVLINK_STX_MAVLINK1 && b != MAVLINK_STX) { /* we have a non-mavlink packet at the start of the buffer. Look ahead for a MAVLink start byte, up to 256 bytes @@ -371,7 +369,7 @@ bool UARTDriver::_write_pending_bytes(void) uint16_t limit = n>256?256:n; uint16_t i; for (i=0; i 0 && b > 0 && + b = _writebuf.peek(0); + if (_packetise && n > 0 && (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX)) { uint8_t min_length = (b == MAVLINK_STX_MAVLINK1)?8:12; // this looks like a MAVLink packet - try to write on diff --git a/libraries/AP_HAL_PX4/UARTDriver.cpp b/libraries/AP_HAL_PX4/UARTDriver.cpp index 1abcc5e011..bd55823b64 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.cpp +++ b/libraries/AP_HAL_PX4/UARTDriver.cpp @@ -14,7 +14,6 @@ #include #include #include -#include #include "GPIO.h" using namespace PX4; @@ -70,19 +69,14 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) thrashing of the heap once we are up. The ttyACM0 driver may not connect for some time after boot */ - if (rxS != 0 && rxS != _readbuf_size) { + if (rxS != 0 && rxS != _readbuf.get_size()) { _initialised = false; while (_in_timer) { hal.scheduler->delay(1); } - _readbuf_size = rxS; - if (_readbuf != NULL) { - free(_readbuf); - } - _readbuf = (uint8_t *)malloc(_readbuf_size); - _readbuf_head = 0; - _readbuf_tail = 0; - } + + _readbuf.set_size(rxS); + } if (b != 0) { _baudrate = b; @@ -91,19 +85,13 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) /* allocate the write buffer */ - if (txS != 0 && txS != _writebuf_size) { + if (txS != 0 && txS != _writebuf.get_size()) { _initialised = false; while (_in_timer) { hal.scheduler->delay(1); } - _writebuf_size = txS; - if (_writebuf != NULL) { - free(_writebuf); - } - _writebuf = (uint8_t *)malloc(_writebuf_size+16); - _writebuf_head = 0; - _writebuf_tail = 0; - } + _writebuf.set_size(txS+16); + } if (_fd == -1) { _fd = open(_devpath, O_RDWR); @@ -130,18 +118,17 @@ void PX4UARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) tcsetattr(_fd, TCSANOW, &t); } - if (_writebuf_size != 0 && _readbuf_size != 0 && _fd != -1) { + if (_writebuf.get_size() && _readbuf.get_size() && _fd != -1) { if (!_initialised) { if (strcmp(_devpath, "/dev/ttyACM0") == 0) { ((PX4GPIO *)hal.gpio)->set_usb_connected(); } ::printf("initialised %s OK %u %u\n", _devpath, - (unsigned)_writebuf_size, (unsigned)_readbuf_size); + (unsigned)_writebuf.get_size(), (unsigned)_readbuf.get_size()); } _initialised = true; } _uart_owner_pid = getpid(); - } void PX4UARTDriver::set_flow_control(enum flow_control fcontrol) @@ -201,19 +188,9 @@ void PX4UARTDriver::end() close(_fd); _fd = -1; } - if (_readbuf) { - free(_readbuf); - _readbuf = NULL; - } - if (_writebuf) { - free(_writebuf); - _writebuf = NULL; - } - _readbuf_size = _writebuf_size = 0; - _writebuf_head = 0; - _writebuf_tail = 0; - _readbuf_head = 0; - _readbuf_tail = 0; + + _readbuf.set_size(0); + _writebuf.set_size(0); } void PX4UARTDriver::flush() {} @@ -235,34 +212,33 @@ bool PX4UARTDriver::tx_pending() { return false; } return number of bytes available to be read from the buffer */ uint32_t PX4UARTDriver::available() -{ - if (!_initialised) { +{ + if (!_initialised) { try_initialise(); - return 0; - } - uint16_t _tail; - return BUF_AVAILABLE(_readbuf); + return 0; + } + + return _readbuf.available(); } /* return number of bytes that can be added to the write buffer */ uint32_t PX4UARTDriver::txspace() -{ - if (!_initialised) { +{ + if (!_initialised) { try_initialise(); - return 0; - } - uint16_t _head; - return BUF_SPACE(_writebuf); + return 0; + } + + return _writebuf.space(); } /* read one byte from the read buffer */ -int16_t PX4UARTDriver::read() -{ - uint8_t c; +int16_t PX4UARTDriver::read() +{ if (_uart_owner_pid != getpid()){ return -1; } @@ -270,15 +246,13 @@ int16_t PX4UARTDriver::read() try_initialise(); return -1; } - if (_readbuf == NULL) { - return -1; - } - if (BUF_EMPTY(_readbuf)) { + + uint8_t byte; + if (!_readbuf.read_byte(&byte)) { return -1; } - c = _readbuf[_readbuf_head]; - BUF_ADVANCEHEAD(_readbuf, 1); - return c; + + return byte; } /* @@ -293,17 +267,14 @@ size_t PX4UARTDriver::write(uint8_t c) try_initialise(); return 0; } - uint16_t _head; - while (BUF_SPACE(_writebuf) == 0) { + while (_writebuf.space() == 0) { if (_nonblocking_writes) { return 0; } hal.scheduler->delay(1); } - _writebuf[_writebuf_tail] = c; - BUF_ADVANCETAIL(_writebuf, 1); - return 1; + return _writebuf.write(&c, 1); } /* @@ -331,36 +302,7 @@ size_t PX4UARTDriver::write(const uint8_t *buffer, size_t size) return ret; } - uint16_t _head, space; - space = BUF_SPACE(_writebuf); - if (space == 0) { - return 0; - } - if (size > space) { - size = space; - } - if (_writebuf_tail < _head) { - // perform as single memcpy - assert(_writebuf_tail+size <= _writebuf_size); - memcpy(&_writebuf[_writebuf_tail], buffer, size); - BUF_ADVANCETAIL(_writebuf, size); - return size; - } - - // perform as two memcpy calls - uint16_t n = _writebuf_size - _writebuf_tail; - if (n > size) n = size; - assert(_writebuf_tail+n <= _writebuf_size); - memcpy(&_writebuf[_writebuf_tail], buffer, n); - BUF_ADVANCETAIL(_writebuf, n); - buffer += n; - n = size - n; - if (n > 0) { - assert(_writebuf_tail+n <= _writebuf_size); - memcpy(&_writebuf[_writebuf_tail], buffer, n); - BUF_ADVANCETAIL(_writebuf, n); - } - return size; + return _writebuf.write(buffer, size); } /* @@ -409,7 +351,6 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) } if (ret > 0) { - BUF_ADVANCEHEAD(_writebuf, ret); _last_write_time = AP_HAL::micros64(); _total_written += ret; if (! _first_write_time && _total_written > 5) { @@ -447,7 +388,6 @@ int PX4UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) // discarding bytes, even if this is a blocking port. This // prevents the ttyACM0 port blocking startup if the endpoint // is not connected - BUF_ADVANCEHEAD(_writebuf, n); return n; } return ret; @@ -472,7 +412,6 @@ int PX4UARTDriver::_read_fd(uint8_t *buf, uint16_t n) } } if (ret > 0) { - BUF_ADVANCETAIL(_readbuf, ret); _total_read += ret; } return ret; @@ -486,7 +425,8 @@ int PX4UARTDriver::_read_fd(uint8_t *buf, uint16_t n) */ void PX4UARTDriver::_timer_tick(void) { - uint16_t n; + int ret; + uint32_t n; if (!_initialised) return; @@ -498,44 +438,33 @@ void PX4UARTDriver::_timer_tick(void) _in_timer = true; // write any pending bytes - uint16_t _tail; - n = BUF_AVAILABLE(_writebuf); + n = _writebuf.available(); if (n > 0) { - uint16_t n1 = _writebuf_size - _writebuf_head; + ByteBuffer::IoVec vec[2]; perf_begin(_perf_uart); - if (n1 >= n) { - // do as a single write - _write_fd(&_writebuf[_writebuf_head], n); - } else { - // split into two writes - int ret = _write_fd(&_writebuf[_writebuf_head], n1); - if (ret == n1 && n > n1) { - _write_fd(&_writebuf[_writebuf_head], n - n1); - } + const auto n_vec = _writebuf.peekiovec(vec, n); + for (int i = 0; i < n_vec; i++) { + ret = _write_fd(vec[i].data, (uint16_t)vec[i].len); + if (ret > 0) + _writebuf.advance(ret); } perf_end(_perf_uart); } // try to fill the read buffer - uint16_t _head; - n = BUF_SPACE(_readbuf); - if (n > 0) { - uint16_t n1 = _readbuf_size - _readbuf_tail; - perf_begin(_perf_uart); - if (n1 >= n) { - // one read will do - assert(_readbuf_tail+n <= _readbuf_size); - _read_fd(&_readbuf[_readbuf_tail], n); - } else { - assert(_readbuf_tail+n1 <= _readbuf_size); - int ret = _read_fd(&_readbuf[_readbuf_tail], n1); - if (ret == n1 && n > n1) { - assert(_readbuf_tail+(n-n1) <= _readbuf_size); - _read_fd(&_readbuf[_readbuf_tail], n - n1); - } - } - perf_end(_perf_uart); + ByteBuffer::IoVec vec[2]; + + perf_begin(_perf_uart); + const auto n_vec = _readbuf.reserve(vec, _readbuf.space()); + for (int i = 0; i < n_vec; i++) { + ret = _read_fd(vec[i].data, vec[i].len); + if (ret < 0) + break; + _readbuf.commit((unsigned)ret); + if ((unsigned)ret < vec[i].len) + break; } + perf_end(_perf_uart); _in_timer = false; } diff --git a/libraries/AP_HAL_PX4/UARTDriver.h b/libraries/AP_HAL_PX4/UARTDriver.h index a0055485ae..18aa4a2c62 100644 --- a/libraries/AP_HAL_PX4/UARTDriver.h +++ b/libraries/AP_HAL_PX4/UARTDriver.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "AP_HAL_PX4.h" #include @@ -48,18 +50,8 @@ private: // we use in-task ring buffers to reduce the system call cost // of ::read() and ::write() in the main loop - uint8_t *_readbuf; - uint16_t _readbuf_size; - - // _head is where the next available data is. _tail is where new - // data is put - volatile uint16_t _readbuf_head; - volatile uint16_t _readbuf_tail; - - uint8_t *_writebuf; - uint16_t _writebuf_size; - volatile uint16_t _writebuf_head; - volatile uint16_t _writebuf_tail; + ByteBuffer _readbuf{0}; + ByteBuffer _writebuf{0}; perf_counter_t _perf_uart; int _write_fd(const uint8_t *buf, uint16_t n);