From ce81c45f80175087c1a953de092a78c8963a183d Mon Sep 17 00:00:00 2001 From: Murilo Belluzzo Date: Fri, 29 Jul 2016 21:40:56 -0300 Subject: [PATCH] AP_HAL_Linux: 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/RPIOUARTDriver.cpp | 71 +------- libraries/AP_HAL_Linux/SPIUARTDriver.cpp | 68 +------ libraries/AP_HAL_Linux/UARTDriver.cpp | 206 ++++++---------------- libraries/AP_HAL_Linux/UARTDriver.h | 17 +- 4 files changed, 68 insertions(+), 294 deletions(-) diff --git a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp index c4a93ecaa2..85a2e46271 100644 --- a/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/RPIOUARTDriver.cpp @@ -4,7 +4,6 @@ #include #include -#include #include "px4io_protocol.h" @@ -34,8 +33,6 @@ RPIOUARTDriver::RPIOUARTDriver() : _need_set_baud(false), _baudrate(0) { - _readbuf = NULL; - _writebuf = NULL; } bool RPIOUARTDriver::sem_take_nonblocking() @@ -75,31 +72,8 @@ void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) _initialised = false; while (_in_timer) hal.scheduler->delay(1); - /* - allocate the read buffer - */ - if (rxS != 0 && rxS != _readbuf_size) { - _readbuf_size = rxS; - if (_readbuf != NULL) { - free(_readbuf); - } - _readbuf = (uint8_t *)malloc(_readbuf_size); - _readbuf_head = 0; - _readbuf_tail = 0; - } - - /* - allocate the write buffer - */ - if (txS != 0 && txS != _writebuf_size) { - _writebuf_size = txS; - if (_writebuf != NULL) { - free(_writebuf); - } - _writebuf = (uint8_t *)malloc(_writebuf_size); - _writebuf_head = 0; - _writebuf_tail = 0; - } + _readbuf.set_size(rxS); + _writebuf.set_size(txS); _dev = hal.spi->get_device("raspio"); @@ -110,10 +84,9 @@ void RPIOUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) hal.scheduler->delay(1); } - if (_writebuf_size != 0 && _readbuf_size != 0) { + if (_writebuf.get_size() && _readbuf.get_size()) { _initialised = true; } - } int RPIOUARTDriver::_write_fd(const uint8_t *buf, uint16_t n) @@ -150,7 +123,7 @@ void RPIOUARTDriver::_timer_tick(void) return; } - struct IOPacket _dma_packet_tx, _dma_packet_rx; + struct IOPacket _dma_packet_tx = {0}, _dma_packet_rx = {0}; _dma_packet_tx.count_code = 2 | PKT_CODE_WRITE; _dma_packet_tx.page = PX4IO_PAGE_UART_BUFFER; @@ -186,11 +159,10 @@ void RPIOUARTDriver::_timer_tick(void) return; } - struct IOPacket _dma_packet_tx, _dma_packet_rx; + struct IOPacket _dma_packet_tx = {0}, _dma_packet_rx = {0}; /* get write_buf bytes */ - uint16_t _tail; - uint16_t n = BUF_AVAILABLE(_writebuf); + uint32_t n = _writebuf.available(); if (n > PKT_MAX_REGS * 2) { n = PKT_MAX_REGS * 2; @@ -201,19 +173,7 @@ void RPIOUARTDriver::_timer_tick(void) n = _max_size; } - if (n > 0) { - uint16_t n1 = _writebuf_size - _writebuf_head; - if (n1 >= n) { - // do as a single write - memcpy( &((uint8_t *)_dma_packet_tx.regs)[0], &_writebuf[_writebuf_head], n ); - } else { - // split into two writes - memcpy( &((uint8_t *)_dma_packet_tx.regs)[0], &_writebuf[_writebuf_head], n1 ); - memcpy( &((uint8_t *)_dma_packet_tx.regs)[n1], &_writebuf[0], n-n1 ); - } - - BUF_ADVANCEHEAD(_writebuf, n); - } + _writebuf.read(&((uint8_t *)_dma_packet_tx.regs)[0], n); _dma_packet_tx.count_code = PKT_MAX_REGS | PKT_CODE_SPIUART; _dma_packet_tx.page = PX4IO_PAGE_UART_BUFFER; @@ -244,8 +204,7 @@ void RPIOUARTDriver::_timer_tick(void) _dev->get_semaphore()->give(); /* add bytes to read buf */ - uint16_t _head; - n = BUF_SPACE(_readbuf); + n = _readbuf.space(); if (_dma_packet_rx.page == PX4IO_PAGE_UART_BUFFER) { @@ -257,19 +216,7 @@ void RPIOUARTDriver::_timer_tick(void) n = PKT_MAX_REGS * 2; } - if (n > 0) { - uint16_t n1 = _readbuf_size - _readbuf_tail; - if (n1 >= n) { - // one read will do - memcpy( &_readbuf[_readbuf_tail], &((uint8_t *)_dma_packet_rx.regs)[0], n ); - } else { - memcpy( &_readbuf[_readbuf_tail], &((uint8_t *)_dma_packet_rx.regs)[0], n1 ); - memcpy( &_readbuf[0], &((uint8_t *)_dma_packet_rx.regs)[n1], n-n1 ); - } - - BUF_ADVANCETAIL(_readbuf, n); - } - + _readbuf.write(&((uint8_t *)_dma_packet_rx.regs)[0], n); } _in_timer = false; diff --git a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp index f0d31cb931..69d30a8d3e 100644 --- a/libraries/AP_HAL_Linux/SPIUARTDriver.cpp +++ b/libraries/AP_HAL_Linux/SPIUARTDriver.cpp @@ -5,7 +5,6 @@ #include #include -#include extern const AP_HAL::HAL &hal; @@ -49,31 +48,8 @@ void SPIUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) txS = 2048; } - /* - allocate the read buffer - */ - if (rxS != 0 && rxS != _readbuf_size) { - _readbuf_size = rxS; - if (_readbuf != NULL) { - free(_readbuf); - } - _readbuf = (uint8_t *)malloc(_readbuf_size); - _readbuf_head = 0; - _readbuf_tail = 0; - } - - /* - allocate the write buffer - */ - if (txS != 0 && txS != _writebuf_size) { - _writebuf_size = txS; - if (_writebuf != NULL) { - free(_writebuf); - } - _writebuf = (uint8_t *)malloc(_writebuf_size); - _writebuf_head = 0; - _writebuf_tail = 0; - } + _readbuf.set_size(rxS); + _writebuf.set_size(txS); if (_buffer == NULL) { /* Do not allocate new buffer, if we're just changing speed */ @@ -122,8 +98,6 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size) _dev->get_semaphore()->give(); - BUF_ADVANCEHEAD(_writebuf, size); - uint16_t ret = size; /* Since all SPI-transactions are transfers we need update @@ -131,41 +105,7 @@ int SPIUARTDriver::_write_fd(const uint8_t *buf, uint16_t size) * this operation since it's the same as in the * UARTDriver::write(). */ - - uint8_t *buffer = _buffer; - - uint16_t _head, space; - space = BUF_SPACE(_readbuf); - - if (space == 0) { - return ret; - } - - if (size > space) { - size = space; - } - - if (_readbuf_tail < _head) { - // perform as single memcpy - assert(_readbuf_tail+size <= _readbuf_size); - memcpy(&_readbuf[_readbuf_tail], buffer, size); - BUF_ADVANCETAIL(_readbuf, size); - return ret; - } - - // perform as two memcpy calls - uint16_t n = _readbuf_size - _readbuf_tail; - if (n > size) n = size; - assert(_readbuf_tail+n <= _readbuf_size); - memcpy(&_readbuf[_readbuf_tail], buffer, n); - BUF_ADVANCETAIL(_readbuf, n); - buffer += n; - n = size - n; - if (n > 0) { - assert(_readbuf_tail+n <= _readbuf_size); - memcpy(&_readbuf[_readbuf_tail], buffer, n); - BUF_ADVANCETAIL(_readbuf, n); - } + _readbuf.write(_buffer, size); return ret; } @@ -194,8 +134,6 @@ int SPIUARTDriver::_read_fd(uint8_t *buf, uint16_t n) _dev->transfer_fullduplex(ff_stub, buf, n); _dev->get_semaphore()->give(); - BUF_ADVANCETAIL(_readbuf, n); - return n; } diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index 89946c6f79..d25d6b0933 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -18,7 +18,6 @@ #include #include -#include #include "ConsoleDevice.h" #include "TCPServerDevice.h" @@ -105,54 +104,15 @@ void UARTDriver::_allocate_buffers(uint16_t rxS, uint16_t txS) txS = 32000; } - /* - allocate the read buffer - */ - if (rxS != 0 && rxS != _readbuf_size) { - _readbuf_size = rxS; - if (_readbuf != NULL) { - free(_readbuf); - } - _readbuf = (uint8_t *)malloc(_readbuf_size); - _readbuf_head = 0; - _readbuf_tail = 0; - } - - /* - allocate the write buffer - */ - if (txS != 0 && txS != _writebuf_size) { - _writebuf_size = txS; - if (_writebuf != NULL) { - free(_writebuf); - } - _writebuf = (uint8_t *)malloc(_writebuf_size); - _writebuf_head = 0; - _writebuf_tail = 0; - } - - if (_writebuf_size != 0 && _readbuf_size != 0) { + if (_writebuf.set_size(txS) && _readbuf.set_size(rxS)) { _initialised = true; } } void UARTDriver::_deallocate_buffers() { - 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); } /* @@ -283,7 +243,7 @@ void UARTDriver::set_blocking_writes(bool blocking) */ bool UARTDriver::tx_pending() { - return !BUF_EMPTY(_writebuf); + return (_writebuf.available() > 0); } /* @@ -294,8 +254,7 @@ uint32_t UARTDriver::available() if (!_initialised) { return 0; } - uint16_t _tail; - return BUF_AVAILABLE(_readbuf); + return _readbuf.available(); } /* @@ -306,22 +265,21 @@ uint32_t UARTDriver::txspace() if (!_initialised) { return 0; } - uint16_t _head; - return BUF_SPACE(_writebuf); + return _writebuf.space(); } int16_t UARTDriver::read() { - uint8_t c; - if (!_initialised || _readbuf == NULL) { + if (!_initialised) { 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; } /* Linux implementations of Print virtual methods */ @@ -330,17 +288,14 @@ size_t UARTDriver::write(uint8_t c) if (!_initialised) { 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); } /* @@ -363,36 +318,7 @@ size_t UARTDriver::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); } /* @@ -400,8 +326,6 @@ size_t UARTDriver::write(const uint8_t *buffer, size_t size) */ int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) { - int ret = 0; - /* allow for delayed connection. This allows ArduPilot to start before a network interface is available. @@ -413,14 +337,7 @@ int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) return 0; } - ret = _device->write(buf, n); - - if (ret > 0) { - BUF_ADVANCEHEAD(_writebuf, ret); - return ret; - } - - return ret; + return _device->write(buf, n); } /* @@ -428,15 +345,7 @@ int UARTDriver::_write_fd(const uint8_t *buf, uint16_t n) */ int UARTDriver::_read_fd(uint8_t *buf, uint16_t n) { - int ret; - - ret = _device->read(buf, n); - - if (ret > 0) { - BUF_ADVANCETAIL(_readbuf, ret); - } - - return ret; + return _device->read(buf, n); } @@ -449,12 +358,11 @@ bool UARTDriver::_write_pending_bytes(void) uint16_t n; // write any pending bytes - uint16_t _tail; - uint16_t available_bytes = BUF_AVAILABLE(_writebuf); + uint32_t available_bytes = _writebuf.available(); n = available_bytes; if (_packetise && n > 0 && - (_writebuf[_writebuf_head] != MAVLINK_STX_MAVLINK1 && - _writebuf[_writebuf_head] != MAVLINK_STX)) { + (_writebuf.peek(0) != MAVLINK_STX_MAVLINK1 && + _writebuf.peek(0) != 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 @@ -463,7 +371,7 @@ bool UARTDriver::_write_pending_bytes(void) uint16_t limit = n>256?256:n; uint16_t i; for (i=0; i 0 && + const int16_t b = _writebuf.peek(0); + if (_packetise && n > 0 && b > 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 @@ -487,10 +395,10 @@ bool UARTDriver::_write_pending_bytes(void) // the length of the packet is the 2nd byte, and mavlink // packets have a 6 byte header plus 2 byte checksum, // giving len+8 bytes - uint8_t len = _writebuf[(_writebuf_head + 1) % _writebuf_size]; + int16_t len = _writebuf.peek(1); if (b == MAVLINK_STX) { // check for signed packet with extra 13 bytes - uint8_t incompat_flags = _writebuf[(_writebuf_head + 2) % _writebuf_size]; + int16_t incompat_flags = _writebuf.peek(2); if (incompat_flags & MAVLINK_IFLAG_SIGNED) { min_length += MAVLINK_SIGNATURE_BLOCK_LEN; } @@ -507,30 +415,27 @@ bool UARTDriver::_write_pending_bytes(void) } if (n > 0) { - uint16_t n1 = _writebuf_size - _writebuf_head; - if (n1 >= n) { - // do as a single write - _write_fd(&_writebuf[_writebuf_head], n); + int ret; + + if (_packetise) { + // keep as a single UDP packet + uint8_t tmpbuf[n]; + _writebuf.peekbytes(tmpbuf, n); + ret = _write_fd(tmpbuf, n); + if (ret > 0) + _writebuf.advance(ret); } else { - // split into two writes - if (_packetise) { - // keep as a single UDP packet - uint8_t tmpbuf[n]; - memcpy(tmpbuf, &_writebuf[_writebuf_head], n1); - if (n > n1) { - memcpy(&tmpbuf[n1], &_writebuf[0], n-n1); - } - _write_fd(tmpbuf, n); - } else { - int ret = _write_fd(&_writebuf[_writebuf_head], n1); - if (ret == n1 && n > n1) { - _write_fd(&_writebuf[_writebuf_head], n - n1); - } + ByteBuffer::IoVec vec[2]; + 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); } } } - return BUF_AVAILABLE(_writebuf) != available_bytes; + return _writebuf.available() != available_bytes; } /* @@ -540,8 +445,6 @@ bool UARTDriver::_write_pending_bytes(void) */ void UARTDriver::_timer_tick(void) { - uint16_t n; - if (!_initialised) return; _in_timer = true; @@ -552,22 +455,17 @@ void UARTDriver::_timer_tick(void) } // try to fill the read buffer - uint16_t _head; - n = BUF_SPACE(_readbuf); - if (n > 0) { - uint16_t n1 = _readbuf_size - _readbuf_tail; - 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); - } - } + int ret; + ByteBuffer::IoVec vec[2]; + + 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; } _in_timer = false; diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index 0cdd83383d..493e495b2a 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -1,6 +1,7 @@ #pragma once #include +#include #include "AP_HAL_Linux.h" #include "SerialDevice.h" @@ -68,24 +69,14 @@ private: protected: const char *device_path; volatile bool _initialised; + // 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}; virtual int _write_fd(const uint8_t *buf, uint16_t n); virtual int _read_fd(uint8_t *buf, uint16_t n); - }; }