diff --git a/platforms/common/Serial.cpp b/platforms/common/Serial.cpp index 2f93a66a6e..28e2d5986b 100644 --- a/platforms/common/Serial.cpp +++ b/platforms/common/Serial.cpp @@ -36,6 +36,10 @@ namespace device { +Serial::Serial() : + _impl(nullptr, 57600, ByteSize::EightBits, Parity::None, StopBits::One, FlowControl::Disabled) {} + + Serial::Serial(const char *port, uint32_t baudrate, ByteSize bytesize, Parity parity, StopBits stopbits, FlowControl flowcontrol) : _impl(port, baudrate, bytesize, parity, stopbits, flowcontrol) @@ -135,4 +139,14 @@ const char *Serial::getPort() const return _impl.getPort(); } +bool Serial::validatePort(const char *port) +{ + return SerialImpl::validatePort(port); +} + +bool Serial::setPort(const char *port) +{ + return _impl.setPort(port); +} + } // namespace device diff --git a/platforms/common/include/px4_platform_common/Serial.hpp b/platforms/common/include/px4_platform_common/Serial.hpp index ce02b5ac63..b38ae93f0a 100644 --- a/platforms/common/include/px4_platform_common/Serial.hpp +++ b/platforms/common/include/px4_platform_common/Serial.hpp @@ -48,6 +48,7 @@ namespace device __EXPORT class Serial { public: + Serial(); Serial(const char *port, uint32_t baudrate = 57600, ByteSize bytesize = ByteSize::EightBits, Parity parity = Parity::None, StopBits stopbits = StopBits::One, FlowControl flowcontrol = FlowControl::Disabled); @@ -83,6 +84,8 @@ public: FlowControl getFlowcontrol() const; bool setFlowcontrol(FlowControl flowcontrol); + static bool validatePort(const char *port); + bool setPort(const char *port); const char *getPort() const; private: diff --git a/platforms/nuttx/src/px4/common/SerialImpl.cpp b/platforms/nuttx/src/px4/common/SerialImpl.cpp index 7490dd604c..c5b6593344 100644 --- a/platforms/nuttx/src/px4/common/SerialImpl.cpp +++ b/platforms/nuttx/src/px4/common/SerialImpl.cpp @@ -53,9 +53,8 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P _stopbits(stopbits), _flowcontrol(flowcontrol) { - if (port) { - strncpy(_port, port, sizeof(_port) - 1); - _port[sizeof(_port) - 1] = '\0'; + if (validatePort(port)) { + setPort(port); } else { _port[0] = 0; @@ -192,6 +191,11 @@ bool SerialImpl::open() return true; } + if (!validatePort(_port)) { + PX4_ERR("Invalid port %s", _port); + return false; + } + // Open the serial port int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); @@ -324,6 +328,27 @@ const char *SerialImpl::getPort() const return _port; } +bool SerialImpl::validatePort(const char *port) +{ + return (port && (access(port, R_OK | W_OK) == 0)); +} + +bool SerialImpl::setPort(const char *port) +{ + if (_open) { + PX4_ERR("Cannot set port after port has already been opened"); + return false; + } + + if (validatePort(port)) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + return true; + } + + return false; +} + uint32_t SerialImpl::getBaudrate() const { return _baudrate; diff --git a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp index 58d41bf759..28d838354e 100644 --- a/platforms/nuttx/src/px4/common/include/SerialImpl.hpp +++ b/platforms/nuttx/src/px4/common/include/SerialImpl.hpp @@ -65,6 +65,8 @@ public: ssize_t write(const void *buffer, size_t buffer_size); const char *getPort() const; + static bool validatePort(const char *port); + bool setPort(const char *port); uint32_t getBaudrate() const; bool setBaudrate(uint32_t baudrate); diff --git a/platforms/posix/include/SerialImpl.hpp b/platforms/posix/include/SerialImpl.hpp index efc95d7d51..d5688834e3 100644 --- a/platforms/posix/include/SerialImpl.hpp +++ b/platforms/posix/include/SerialImpl.hpp @@ -65,6 +65,8 @@ public: ssize_t write(const void *buffer, size_t buffer_size); const char *getPort() const; + static bool validatePort(const char *port); + bool setPort(const char *port); uint32_t getBaudrate() const; bool setBaudrate(uint32_t baudrate); diff --git a/platforms/posix/src/px4/common/SerialImpl.cpp b/platforms/posix/src/px4/common/SerialImpl.cpp index 79e3422aed..739796e6a1 100644 --- a/platforms/posix/src/px4/common/SerialImpl.cpp +++ b/platforms/posix/src/px4/common/SerialImpl.cpp @@ -51,9 +51,8 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P _stopbits(stopbits), _flowcontrol(flowcontrol) { - if (port) { - strncpy(_port, port, sizeof(_port) - 1); - _port[sizeof(_port) - 1] = '\0'; + if (validatePort(port)) { + setPort(port); } else { _port[0] = 0; @@ -190,6 +189,11 @@ bool SerialImpl::open() return true; } + if (!validatePort(_port)) { + PX4_ERR("Invalid port %s", _port); + return false; + } + // Open the serial port int serial_fd = ::open(_port, O_RDWR | O_NOCTTY); @@ -317,6 +321,27 @@ const char *SerialImpl::getPort() const return _port; } +bool SerialImpl::validatePort(const char *port) +{ + return (port && (access(port, R_OK | W_OK) == 0)); +} + +bool SerialImpl::setPort(const char *port) +{ + if (_open) { + PX4_ERR("Cannot set port after port has already been opened"); + return false; + } + + if (validatePort(port)) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + return true; + } + + return false; +} + uint32_t SerialImpl::getBaudrate() const { return _baudrate; diff --git a/platforms/qurt/include/SerialImpl.hpp b/platforms/qurt/include/SerialImpl.hpp index 1b98d3bb40..91bbde0464 100644 --- a/platforms/qurt/include/SerialImpl.hpp +++ b/platforms/qurt/include/SerialImpl.hpp @@ -65,6 +65,7 @@ public: const char *getPort() const; bool setPort(const char *port); + static bool validatePort(const char *port); uint32_t getBaudrate() const; bool setBaudrate(uint32_t baudrate); diff --git a/platforms/qurt/src/px4/SerialImpl.cpp b/platforms/qurt/src/px4/SerialImpl.cpp index ec0fb73fce..287064b760 100644 --- a/platforms/qurt/src/px4/SerialImpl.cpp +++ b/platforms/qurt/src/px4/SerialImpl.cpp @@ -48,9 +48,8 @@ SerialImpl::SerialImpl(const char *port, uint32_t baudrate, ByteSize bytesize, P _stopbits(stopbits), _flowcontrol(flowcontrol) { - if (port) { - strncpy(_port, port, sizeof(_port) - 1); - _port[sizeof(_port) - 1] = '\0'; + if (validatePort(port)) { + setPort(port); } else { _port[0] = 0; @@ -117,6 +116,11 @@ bool SerialImpl::open() return false; } + if (!validatePort(_port)) { + PX4_ERR("Invalid port %s", _port); + return false; + } + // qurt_uart_open will check validity of port and baudrate int serial_fd = qurt_uart_open(_port, _baudrate); @@ -256,6 +260,27 @@ const char *SerialImpl::getPort() const return _port; } +bool SerialImpl::validatePort(const char *port) +{ + return (qurt_uart_get_port(port) >= 0); +} + +bool SerialImpl::setPort(const char *port) +{ + if (_open) { + PX4_ERR("Cannot set port after port has already been opened"); + return false; + } + + if (validatePort(port)) { + strncpy(_port, port, sizeof(_port) - 1); + _port[sizeof(_port) - 1] = '\0'; + return true; + } + + return false; +} + uint32_t SerialImpl::getBaudrate() const { return _baudrate; diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 766be323d7..aa8b8d7543 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -174,7 +174,7 @@ private: #ifdef __PX4_LINUX int _spi_fd {-1}; ///< SPI interface to GPS #endif - Serial *_uart = nullptr; ///< UART interface to GPS + Serial _uart {}; ///< UART interface to GPS unsigned _baudrate{0}; ///< current baudrate const unsigned _configured_baudrate{0}; ///< configured baudrate (0=auto-detect) char _port[20] {}; ///< device / serial port path @@ -416,8 +416,8 @@ int GPS::callback(GPSCallbackType type, void *data1, int data2, void *user) int ret = 0; - if (gps->_uart) { - ret = gps->_uart->write((void *) data1, (size_t) data2); + if (gps->_interface == GPSHelper::Interface::UART) { + ret = gps->_uart.write((void *) data1, (size_t) data2); #ifdef __PX4_LINUX @@ -477,8 +477,8 @@ int GPS::pollOrRead(uint8_t *buf, size_t buf_length, int timeout) handleInjectDataTopic(); - if ((_interface == GPSHelper::Interface::UART) && (_uart)) { - ret = _uart->readAtLeast(buf, buf_length, character_count, timeout_adjusted); + if (_interface == GPSHelper::Interface::UART) { + ret = _uart.readAtLeast(buf, buf_length, character_count, timeout_adjusted); // SPI is only supported on LInux #if defined(__PX4_LINUX) @@ -598,8 +598,8 @@ bool GPS::injectData(uint8_t *data, size_t len) size_t written = 0; - if ((_interface == GPSHelper::Interface::UART) && (_uart)) { - written = _uart->write((const void *) data, len); + if (_interface == GPSHelper::Interface::UART) { + written = _uart.write((const void *) data, len); #ifdef __PX4_LINUX @@ -615,7 +615,7 @@ bool GPS::injectData(uint8_t *data, size_t len) int GPS::setBaudrate(unsigned baud) { if (_interface == GPSHelper::Interface::UART) { - if ((_uart) && (_uart->setBaudrate(baud))) { + if (_uart.setBaudrate(baud)) { return 0; } @@ -786,23 +786,19 @@ GPS::run() _helper = nullptr; } - if ((_interface == GPSHelper::Interface::UART) && (_uart == nullptr)) { + if ((_interface == GPSHelper::Interface::UART) && (! _uart.isOpen())) { - // Create the UART port instance - _uart = new Serial(_port); - - if (_uart == nullptr) { - PX4_ERR("Error creating serial device %s", _port); + // Configure UART port + if (!_uart.setPort(_port)) { + PX4_ERR("Error configuring serial device on port %s", _port); px4_sleep(1); continue; } - } - if ((_interface == GPSHelper::Interface::UART) && (! _uart->isOpen())) { // Configure the desired baudrate if one was specified by the user. // Otherwise the default baudrate will be used. if (_configured_baudrate) { - if (! _uart->setBaudrate(_configured_baudrate)) { + if (! _uart.setBaudrate(_configured_baudrate)) { PX4_ERR("Error setting baudrate to %u on %s", _configured_baudrate, _port); px4_sleep(1); continue; @@ -810,7 +806,7 @@ GPS::run() } // Open the UART. If this is successful then the UART is ready to use. - if (! _uart->open()) { + if (! _uart.open()) { PX4_ERR("Error opening serial device %s", _port); px4_sleep(1); continue; @@ -1029,10 +1025,8 @@ GPS::run() } } - if ((_interface == GPSHelper::Interface::UART) && (_uart)) { - (void) _uart->close(); - delete _uart; - _uart = nullptr; + if (_interface == GPSHelper::Interface::UART) { + (void) _uart.close(); #ifdef __PX4_LINUX @@ -1528,7 +1522,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) GPS *gps = nullptr; if (instance == Instance::Main) { - if (device_name && (access(device_name, R_OK|W_OK) == 0)) { + if (Serial::validatePort(device_name)) { gps = new GPS(device_name, mode, interface, instance, baudrate_main); } else { @@ -1551,7 +1545,7 @@ GPS *GPS::instantiate(int argc, char *argv[], Instance instance) } } } else { // secondary instance - if (device_name_secondary && (access(device_name_secondary, R_OK|W_OK) == 0)) { + if (Serial::validatePort(device_name_secondary)) { gps = new GPS(device_name_secondary, mode, interface_secondary, instance, baudrate_secondary); } else { diff --git a/src/lib/drivers/device/qurt/uart.c b/src/lib/drivers/device/qurt/uart.c index 798d205344..ca1d0f66c7 100644 --- a/src/lib/drivers/device/qurt/uart.c +++ b/src/lib/drivers/device/qurt/uart.c @@ -25,19 +25,32 @@ void configure_uart_callbacks(open_uart_func_t open_func, } } -int qurt_uart_open(const char *dev, speed_t speed) +int qurt_uart_get_port(const char *dev) { - if (_callbacks_configured) { + if (dev != NULL) { // Convert device string into a uart port number char *endptr = NULL; - uint8_t port_number = (uint8_t) strtol(dev, &endptr, 10); + int port_number = strtol(dev, &endptr, 10); if ((port_number == 0) && (endptr == dev)) { PX4_ERR("Could not convert %s into a valid uart port number", dev); return -1; - } - return _open_uart(port_number, speed); + } else { + return port_number; + } + } + + return -1; +} + +int qurt_uart_open(const char *dev, speed_t speed) +{ + int port_number = qurt_uart_get_port(dev); + + if (_callbacks_configured && (port_number >= 0)) { + + return _open_uart((uint8_t) port_number, speed); } else { PX4_ERR("Cannot open uart until callbacks have been configured"); diff --git a/src/lib/drivers/device/qurt/uart.h b/src/lib/drivers/device/qurt/uart.h index 0305596189..d1632fe6a2 100644 --- a/src/lib/drivers/device/qurt/uart.h +++ b/src/lib/drivers/device/qurt/uart.h @@ -7,6 +7,7 @@ extern "C" { #include #include +int qurt_uart_get_port(const char *dev); int qurt_uart_open(const char *dev, speed_t speed); int qurt_uart_write(int fd, const char *buf, size_t len); int qurt_uart_read(int fd, char *buf, size_t len, uint32_t timeout_us);