Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Eliminate write(c) from UARTDrivers #12643

Open
wants to merge 9 commits into
base: master
from
@@ -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;
This conversation was marked as resolved by peterbarker

This comment has been minimized.

Copy link
@WickedShell

WickedShell Oct 27, 2019

Contributor
Suggested change
const size_t bytes_to_copy = (space_remaining < size) ? space_remaining : size;
const size_t bytes_to_copy = MIN(space_remaining, size);

This comment has been minimized.

Copy link
@peterbarker

peterbarker Oct 28, 2019

Author Contributor

So after initially applying this, I've reverted it as it would require including AP_Math.h, probably not something to do here.

memcpy(&_str[_offs], buffer, bytes_to_copy);
}
return n;
_offs += size;
return size;
}

size_t _offs;
@@ -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);

@@ -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;
}
@@ -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;
@@ -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
@@ -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;
}
@@ -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;
};
@@ -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;
}
@@ -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;
@@ -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) {
@@ -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};

ProTip! Use n and p to navigate between commits in a pull request.
You can’t perform that action at this time.