HAL_Linux: added buffering on the UARTs

This commit is contained in:
Andrew Tridgell 2013-09-29 10:13:51 +10:00
parent 605e6c3cf7
commit 1e786b3e20
3 changed files with 325 additions and 66 deletions

View File

@ -4,6 +4,7 @@
#include "Scheduler.h" #include "Scheduler.h"
#include "Storage.h" #include "Storage.h"
#include "UARTDriver.h"
#include <unistd.h> #include <unistd.h>
#include <sys/time.h> #include <sys/time.h>
#include <poll.h> #include <poll.h>
@ -203,7 +204,10 @@ void *LinuxScheduler::_uart_thread(void)
while (true) { while (true) {
poll(NULL, 0, 1); poll(NULL, 0, 1);
// process any pending serial bytes: TODO // process any pending serial bytes
((LinuxUARTDriver *)hal.uartA)->_timer_tick();
((LinuxUARTDriver *)hal.uartB)->_timer_tick();
((LinuxUARTDriver *)hal.uartC)->_timer_tick();
} }
return NULL; return NULL;
} }

View File

@ -12,8 +12,12 @@
#include <sys/stat.h> #include <sys/stat.h>
#include <fcntl.h> #include <fcntl.h>
#include <unistd.h> #include <unistd.h>
#include <poll.h>
#include <assert.h>
#include <sys/ioctl.h> #include <sys/ioctl.h>
extern const AP_HAL::HAL& hal;
using namespace Linux; using namespace Linux;
LinuxUARTDriver::LinuxUARTDriver() : LinuxUARTDriver::LinuxUARTDriver() :
@ -35,36 +39,96 @@ void LinuxUARTDriver::set_device_path(const char *path)
*/ */
void LinuxUARTDriver::begin(uint32_t b) void LinuxUARTDriver::begin(uint32_t b)
{ {
if (device_path == NULL) { begin(b, 0, 0);
return;
}
if (_fd == -1) {
_fd = open(device_path, O_RDWR);
if (_fd == -1) {
::printf("UARTDriver: Failed to open %s - %s\n",
device_path,
strerror(errno));
return;
}
}
/* if baudrate has been specified, then set the baudrate */
if (b != 0) {
struct termios t;
tcgetattr(_fd, &t);
cfsetspeed(&t, b);
// disable LF -> CR/LF
t.c_oflag &= ~ONLCR;
tcsetattr(_fd, TCSANOW, &t);
}
} }
void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
{ {
// ignore buffer sizes for now if (!_initialised) {
begin(b); if (device_path == NULL) {
return;
}
uint8_t retries = 0;
while (retries < 5) {
_fd = open(device_path, O_RDWR);
if (_fd != -1) {
break;
}
// sleep a bit and retry. There seems to be a NuttX bug
// that can cause ttyACM0 to not be available immediately,
// but a small delay can fix it
hal.scheduler->delay(100);
retries++;
}
if (_fd == -1) {
fprintf(stdout, "Failed to open UART device %s - %s\n",
device_path, strerror(errno));
return;
}
if (retries != 0) {
fprintf(stdout, "WARNING: took %u retries to open UART %s\n",
(unsigned)retries, device_path);
return;
}
// always run the file descriptor non-blocking, and deal with
// blocking IO in the higher level calls
fcntl(_fd, F_SETFL, fcntl(_fd, F_GETFL, 0) | O_NONBLOCK);
if (rxS == 0) {
rxS = 128;
}
// we have enough memory to have a larger transmit buffer for
// all ports. This means we don't get delays while waiting to
// write GPS config packets
if (txS < 512) {
txS = 512;
}
}
_initialised = false;
while (_in_timer) hal.scheduler->delay(1);
if (b != 0) {
// set the baud rate
struct termios t;
tcgetattr(_fd, &t);
cfsetspeed(&t, b);
// disable LF -> CR/LF
t.c_oflag &= ~ONLCR;
tcsetattr(_fd, TCSANOW, &t);
}
/*
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+16);
_writebuf_head = 0;
_writebuf_tail = 0;
}
if (_writebuf_size != 0 && _readbuf_size != 0) {
_initialised = true;
}
} }
/* /*
@ -72,10 +136,25 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS)
*/ */
void LinuxUARTDriver::end() void LinuxUARTDriver::end()
{ {
_initialised = false;
while (_in_timer) hal.scheduler->delay(1);
if (_fd != -1) { if (_fd != -1) {
close(_fd); close(_fd);
_fd = -1; _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;
} }
@ -90,7 +169,7 @@ void LinuxUARTDriver::flush()
*/ */
bool LinuxUARTDriver::is_initialized() bool LinuxUARTDriver::is_initialized()
{ {
return (_fd != -1); return _initialised;
} }
@ -99,48 +178,37 @@ bool LinuxUARTDriver::is_initialized()
*/ */
void LinuxUARTDriver::set_blocking_writes(bool blocking) void LinuxUARTDriver::set_blocking_writes(bool blocking)
{ {
unsigned v; _nonblocking_writes = !blocking;
if (_fd == -1) {
return;
}
v = fcntl(_fd, F_GETFL, 0);
if (blocking) {
v &= ~O_NONBLOCK;
} else {
v |= O_NONBLOCK;
}
fcntl(_fd, F_SETFL, v);
} }
/*
buffer handling macros
*/
#define BUF_AVAILABLE(buf) ((buf##_head > (_tail=buf##_tail))? (buf##_size - buf##_head) + _tail: _tail - buf##_head)
#define BUF_SPACE(buf) (((_head=buf##_head) > buf##_tail)?(_head - buf##_tail) - 1:((buf##_size - buf##_tail) + _head) - 1)
#define BUF_EMPTY(buf) (buf##_head == buf##_tail)
#define BUF_ADVANCETAIL(buf, n) buf##_tail = (buf##_tail + n) % buf##_size
#define BUF_ADVANCEHEAD(buf, n) buf##_head = (buf##_head + n) % buf##_size
/* /*
do we have any bytes pending transmission? do we have any bytes pending transmission?
*/ */
bool LinuxUARTDriver::tx_pending() bool LinuxUARTDriver::tx_pending()
{ {
// no buffering, so always false return !BUF_EMPTY(_writebuf);
return false;
} }
/* /*
return the number of bytes available to be read return the number of bytes available to be read
*/ */
int16_t LinuxUARTDriver::available() int16_t LinuxUARTDriver::available()
{ {
int nread; if (!_initialised) {
if (_fd == -1) {
return 0; return 0;
} }
uint16_t _tail;
nread = 0; return BUF_AVAILABLE(_readbuf);
if (ioctl(_fd, FIONREAD, (unsigned long)&nread) == 0) {
return nread;
}
// ioctl failed??
return 0;
} }
/* /*
@ -148,33 +216,195 @@ int16_t LinuxUARTDriver::available()
*/ */
int16_t LinuxUARTDriver::txspace() int16_t LinuxUARTDriver::txspace()
{ {
// for now lie and say we always have 128, we will need a ring if (!_initialised) {
// buffer later and a IO thread return 0;
return 128; }
uint16_t _head;
return BUF_SPACE(_writebuf);
} }
int16_t LinuxUARTDriver::read() int16_t LinuxUARTDriver::read()
{ {
char c; uint8_t c;
if (_fd == -1) { if (!_initialised || _readbuf == NULL) {
return -1; return -1;
} }
if (::read(_fd, &c, 1) == 1) { if (BUF_EMPTY(_readbuf)) {
return (int16_t)c; return -1;
} }
return -1; c = _readbuf[_readbuf_head];
BUF_ADVANCEHEAD(_readbuf, 1);
return c;
} }
/* Linux implementations of Print virtual methods */ /* Linux implementations of Print virtual methods */
size_t LinuxUARTDriver::write(uint8_t c) size_t LinuxUARTDriver::write(uint8_t c)
{ {
if (_fd == -1) { if (!_initialised) {
return 0; return 0;
} }
if (::write(_fd, &c, 1) == 1) { if (hal.scheduler->in_timerprocess()) {
return 1; // not allowed from timers
return 0;
} }
return 0; uint16_t _head;
while (BUF_SPACE(_writebuf) == 0) {
if (_nonblocking_writes) {
return 0;
}
hal.scheduler->delay(1);
}
_writebuf[_writebuf_tail] = c;
BUF_ADVANCETAIL(_writebuf, 1);
return 1;
}
/*
write size bytes to the write buffer
*/
size_t LinuxUARTDriver::write(const uint8_t *buffer, size_t size)
{
if (!_initialised) {
return 0;
}
if (hal.scheduler->in_timerprocess()) {
// not allowed from timers
return 0;
}
if (!_nonblocking_writes) {
/*
use the per-byte delay loop in write() above for blocking writes
*/
size_t ret = 0;
while (size--) {
if (write(*buffer++) != 1) break;
ret++;
}
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;
}
/*
try writing n bytes, handling an unresponsive port
*/
int LinuxUARTDriver::_write_fd(const uint8_t *buf, uint16_t n)
{
int ret = 0;
struct pollfd fds;
fds.fd = _fd;
fds.events = POLLOUT;
fds.revents = 0;
if (poll(&fds, 1, 0) == 1) {
ret = ::write(_fd, buf, n);
}
if (ret > 0) {
BUF_ADVANCEHEAD(_writebuf, ret);
return ret;
}
return ret;
}
/*
try reading n bytes, handling an unresponsive port
*/
int LinuxUARTDriver::_read_fd(uint8_t *buf, uint16_t n)
{
int ret;
ret = ::read(_fd, buf, n);
if (ret > 0) {
BUF_ADVANCETAIL(_readbuf, ret);
}
return ret;
}
/*
push any pending bytes to/from the serial port. This is called at
1kHz in the timer thread. Doing it this way reduces the system call
overhead in the main task enormously.
*/
void LinuxUARTDriver::_timer_tick(void)
{
uint16_t n;
if (!_initialised) return;
_in_timer = true;
// write any pending bytes
uint16_t _tail;
n = BUF_AVAILABLE(_writebuf);
if (n > 0) {
if (_tail > _writebuf_head) {
// do as a single write
_write_fd(&_writebuf[_writebuf_head], n);
} else {
// split into two writes
uint16_t n1 = _writebuf_size - _writebuf_head;
int ret = _write_fd(&_writebuf[_writebuf_head], n1);
if (ret == n1 && n != n1) {
_write_fd(&_writebuf[_writebuf_head], n - n1);
}
}
}
// try to fill the read buffer
uint16_t _head;
n = BUF_SPACE(_readbuf);
if (n > 0) {
if (_readbuf_tail < _head) {
// one read will do
assert(_readbuf_tail+n <= _readbuf_size);
_read_fd(&_readbuf[_readbuf_tail], n);
} else {
uint16_t n1 = _readbuf_size - _readbuf_tail;
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);
}
}
}
_in_timer = false;
} }
#endif // CONFIG_HAL_BOARD #endif // CONFIG_HAL_BOARD

View File

@ -23,12 +23,37 @@ public:
/* Linux implementations of Print virtual methods */ /* Linux implementations of Print virtual methods */
size_t write(uint8_t c); size_t write(uint8_t c);
size_t write(const uint8_t *buffer, size_t size);
void set_device_path(const char *path); void set_device_path(const char *path);
void _timer_tick(void);
private: private:
const char *device_path; const char *device_path;
int _fd; int _fd;
bool _nonblocking_writes;
volatile bool _initialised;
volatile bool _in_timer;
// 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;
int _write_fd(const uint8_t *buf, uint16_t n);
int _read_fd(uint8_t *buf, uint16_t n);
uint64_t _last_write_time;
}; };
#endif // __AP_HAL_LINUX_UARTDRIVER_H__ #endif // __AP_HAL_LINUX_UARTDRIVER_H__