From be68de20f1db6232aecdb0b7cd01f8ea4e989bde Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?V=C3=ADctor=20Mayoral=20Vilches?= Date: Wed, 11 Jun 2014 08:34:07 +0200 Subject: [PATCH] HAL_Linux: UART-like TCP sockets impl. on AP_HAL_Linux --- libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 12 +- libraries/AP_HAL_Linux/UARTDriver.cpp | 239 ++++++++++++++++++--- libraries/AP_HAL_Linux/UARTDriver.h | 12 +- 3 files changed, 224 insertions(+), 39 deletions(-) diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index ba3e75123f..92b129d2ae 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -46,6 +46,16 @@ HAL_Linux::HAL_Linux() : &utilInstance) {} +void _usage(void) +{ + printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n"); + printf("Options:\n"); + printf("\t-serial: -A /dev/ttyO4\n"); + printf("\t -B /dev/ttyS1\n"); + printf("\t-tcp: -C tcp:192.168.2.15:1243:wait\n"); + printf("\t -A tcp:11.0.0.2:5678\n"); +} + void HAL_Linux::init(int argc,char* const argv[]) const { int opt; @@ -64,7 +74,7 @@ void HAL_Linux::init(int argc,char* const argv[]) const uartCDriver.set_device_path(optarg); break; case 'h': - printf("Usage: -A uartAPath -B uartBPath -C uartCPath\n"); + _usage(); exit(0); default: printf("Unknown option '%c'\n", (char)opt); diff --git a/libraries/AP_HAL_Linux/UARTDriver.cpp b/libraries/AP_HAL_Linux/UARTDriver.cpp index 7089a92295..15bb8eeab8 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.cpp +++ b/libraries/AP_HAL_Linux/UARTDriver.cpp @@ -15,11 +15,21 @@ #include #include #include +#include +#include +#include +#include +#include +#include extern const AP_HAL::HAL& hal; using namespace Linux; +#define DEVICE_TCP 0 +#define DEVICE_SERIAL 1 +#define DEVICE_UNKNOWN 99 + LinuxUARTDriver::LinuxUARTDriver(bool default_console) : device_path(NULL), _rd_fd(-1), @@ -35,7 +45,7 @@ LinuxUARTDriver::LinuxUARTDriver(bool default_console) : /* set the tty device to use for this UART */ -void LinuxUARTDriver::set_device_path(const char *path) +void LinuxUARTDriver::set_device_path(char *path) { device_path = path; } @@ -61,43 +71,85 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) if (device_path == NULL) { return; } - uint8_t retries = 0; - while (retries < 5) { - _rd_fd = open(device_path, O_RDWR); - if (_rd_fd != -1) { + + switch (_parseDevicePath(device_path)){ + case DEVICE_TCP: + { + _connected = false; + if (_flag != NULL){ + _tcp_start_connection(true); + } else { + _tcp_start_connection(false); + } + + if (_connected) { + if (rxS < 1024) { + rxS = 1024; + } + if (txS < 1024) { + txS = 1024; + } + } else { + printf("LinuxUARTDriver TCP connection not stablished\n"); + exit(1); + } + break; + } + case DEVICE_SERIAL: + { + uint8_t retries = 0; + while (retries < 5) { + _rd_fd = open(device_path, O_RDWR); + if (_rd_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++; + } + _wr_fd = _rd_fd; + if (_rd_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(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK); + + if (rxS < 1024) { + rxS = 1024; + } + + // 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 < 1024) { + txS = 1024; + } 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++; - } - _wr_fd = _rd_fd; - if (_rd_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; - } + default: + { + // Notify that the option is not valid and select standart input and output + printf("LinuxUARTDriver parsing failed, using default\n"); - // always run the file descriptor non-blocking, and deal with - // blocking IO in the higher level calls - fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK); - - if (rxS < 1024) { - rxS = 1024; - } - - // 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 < 1024) { - txS = 1024; + _rd_fd = 0; + _wr_fd = 1; + rxS = 512; + txS = 512; + fcntl(_rd_fd, F_SETFL, fcntl(_rd_fd, F_GETFL, 0) | O_NONBLOCK); + fcntl(_wr_fd, F_SETFL, fcntl(_wr_fd, F_GETFL, 0) | O_NONBLOCK); + break; + } } } @@ -145,12 +197,129 @@ void LinuxUARTDriver::begin(uint32_t b, uint16_t rxS, uint16_t txS) } } +/* + Device path accepts the following syntaxes: + - /dev/ttyO1 + - tcp:192.168.2.15:1243:wait +*/ +int LinuxUARTDriver::_parseDevicePath(char* arg) +{ + const char *serial_string = "/dev/tty"; + const char *tcp_string = "tcp"; + _flag = NULL; // init flag + + if(strstr(arg, tcp_string) != NULL){ + // Parse the TCP string + char *protocol, *ip, *port, *flag; + protocol = strtok ( arg, ":" ); + ip = strtok ( NULL, ":" ); + port = strtok ( NULL, ":" ); + flag = strtok ( NULL, ":" ); + + _base_port = (uint16_t) atoi(port); + _ip = ip; + _flag = flag; + return DEVICE_TCP; + } else if (strstr(arg, serial_string) != NULL){ + return DEVICE_SERIAL; + } else { + return DEVICE_UNKNOWN; + } +} + +/* + start a TCP connection for the serial port. If wait_for_connection + is true then block until a client connects + */ +void LinuxUARTDriver::_tcp_start_connection(bool wait_for_connection) +{ + int one=1; + struct sockaddr_in sockaddr; + int ret; + int listen_fd = -1; // socket we are listening on + int net_fd = -1; // network file descriptor, will be linked to wr_fd and rd_fd + uint8_t portNumber = 1; + + // if (_console) { + // // hack for console access + // connected = true; + // listen_fd = -1; + // fd = 1; + // return; + // } + + if (net_fd != -1) { + close(net_fd); + } + + if (listen_fd == -1) { + memset(&sockaddr,0,sizeof(sockaddr)); + +#ifdef HAVE_SOCK_SIN_LEN + sockaddr.sin_len = sizeof(sockaddr); +#endif + sockaddr.sin_port = htons(_base_port + portNumber); + sockaddr.sin_family = AF_INET; + // sockaddr.sin_addr.s_addr = inet_addr(_base_ip); + + // Bind to all interfaces + sockaddr.sin_addr.s_addr = htonl(INADDR_ANY); + + listen_fd = socket(AF_INET, SOCK_STREAM, 0); + if (listen_fd == -1) { + printf("socket failed - %s\n", strerror(errno)); + exit(1); + } + + /* we want to be able to re-use ports quickly */ + setsockopt(listen_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + + printf("bind port %u for %u\n", + (unsigned)ntohs(sockaddr.sin_port), + (unsigned)portNumber), + + ret = bind(listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); + if (ret == -1) { + printf("bind failed on port %u - %s\n", + (unsigned)ntohs(sockaddr.sin_port), + strerror(errno)); + exit(1); + } + + ret = listen(listen_fd, 5); + if (ret == -1) { + printf("listen failed - %s\n", strerror(errno)); + exit(1); + } + + printf("Serial port %u on TCP port %u\n", portNumber, + _base_port + portNumber); + // fflush(stdout); + } + + if (wait_for_connection) { + printf("Waiting for connection ....\n"); + // fflush(stdout); + net_fd = accept(listen_fd, NULL, NULL); + if (net_fd == -1) { + printf("accept() error - %s", strerror(errno)); + exit(1); + } + setsockopt(net_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); + setsockopt(net_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); + _connected = true; + _rd_fd = net_fd; + _wr_fd = net_fd; + } +} + /* shutdown a UART */ void LinuxUARTDriver::end() { _initialised = false; + _connected = false; while (_in_timer) hal.scheduler->delay(1); if (_rd_fd == _wr_fd && _rd_fd != -1) { close(_rd_fd); diff --git a/libraries/AP_HAL_Linux/UARTDriver.h b/libraries/AP_HAL_Linux/UARTDriver.h index 00326d64e6..a90eb5fd62 100644 --- a/libraries/AP_HAL_Linux/UARTDriver.h +++ b/libraries/AP_HAL_Linux/UARTDriver.h @@ -25,18 +25,22 @@ public: 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(char *path); void _timer_tick(void); private: - const char *device_path; + char *device_path; int _rd_fd; int _wr_fd; bool _nonblocking_writes; bool _console; volatile bool _initialised; volatile bool _in_timer; + uint16_t _base_port; + char *_ip; + char *_flag; + bool _connected; // true if a client has connected // we use in-task ring buffers to reduce the system call cost // of ::read() and ::write() in the main loop @@ -55,7 +59,9 @@ private: int _write_fd(const uint8_t *buf, uint16_t n); int _read_fd(uint8_t *buf, uint16_t n); - uint64_t _last_write_time; + void _tcp_start_connection(bool wait_for_connection); + int _parseDevicePath(char* arg); + uint64_t _last_write_time; }; #endif // __AP_HAL_LINUX_UARTDRIVER_H__