aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_HAL_Linux: UARTDriver: Make use of ByteBuffer class

Browse Source 
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.
This commit is contained in:
Murilo Belluzzo 2016-07-29 21:40:56 -03:00 committed by Andrew Tridgell
parent 8526b25654
commit ce81c45f80
4 changed files with 68 additions and 294 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
libraries/AP_HAL_Linux/RPIOUARTDriver.cpp
View File

@ -4,7 +4,6 @@
#include <cstdio>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
#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;

68
libraries/AP_HAL_Linux/SPIUARTDriver.cpp
View File

@ -5,7 +5,6 @@
#include <cstdio>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
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;
}

206
libraries/AP_HAL_Linux/UARTDriver.cpp
View File

@ -18,7 +18,6 @@
#include <unistd.h>
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/utility/RingBuffer.h>
#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<limit; i++) {
uint8_t b = _writebuf[(_writebuf_head + i) % _writebuf_size];
int16_t b = _writebuf.peek(i);
if (b == MAVLINK_STX_MAVLINK1 || b == MAVLINK_STX) {
n = i;
break;
@ -474,8 +382,8 @@ bool UARTDriver::_write_pending_bytes(void)
n = limit;
}
}
const uint8_t b = _writebuf[_writebuf_head];
if (_packetise && n > 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;

17
libraries/AP_HAL_Linux/UARTDriver.h
View File

@ -1,6 +1,7 @@
#pragma once
#include <AP_HAL/utility/OwnPtr.h>
#include <AP_HAL/utility/RingBuffer.h>
#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);
};
}