// -*- Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- // // Copyright (c) 2010 Michael Smith. All rights reserved. // // This is free software; you can redistribute it and/or modify it under // the terms of the GNU Lesser General Public License as published by the // Free Software Foundation; either version 2.1 of the License, or (at // your option) any later version. // #include #if CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL #include #include #include #include #include #include #include #include #include #include #include #include #include #include "vprintf.h" #include "UARTDriver.h" using namespace AVR_SITL; #define LISTEN_BASE_PORT 5760 bool SITLUARTDriver::_console; /* UARTDriver method implementations */ void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace) { if (txSpace != 0) { _txSpace = txSpace; } if (rxSpace != 0) { _rxSpace = rxSpace; } switch (_portNumber) { case 0: _tcp_start_connection(true); break; case 1: /* gps */ _connected = true; _fd = -1; // sitl_gps_pipe(); break; default: _tcp_start_connection(false); break; } } void SITLUARTDriver::end() { } int16_t SITLUARTDriver::available(void) { _check_connection(); if (!_connected) { return 0; } if (_select_check(_fd)) { #ifdef FIONREAD // use FIONREAD to get exact value if possible int num_ready; if (ioctl(_fd, FIONREAD, &num_ready) == 0) { if (num_ready > _rxSpace) { return _rxSpace; } if (num_ready == 0) { // EOF is reached fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); close(_fd); _connected = false; return 0; } return num_ready; } #endif return 1; // best we can do is say 1 byte available } return 0; } int16_t SITLUARTDriver::txspace(void) { // always claim there is space available return _txSpace; } int16_t SITLUARTDriver::read(void) { char c; if (available() <= 0) { return -1; } #if 0 if (_portNumber == 1) { if (sitl_gps_read(_fd, &c, 1) == 1) { return (uint8_t)c; } return -1; } #endif if (_console) { return ::read(0, &c, 1); } int n = recv(_fd, &c, 1, MSG_DONTWAIT | MSG_NOSIGNAL); if (n <= 0) { // the socket has reached EOF close(_fd); _connected = false; fprintf(stdout, "Closed connection on serial port %u\n", _portNumber); fflush(stdout); return -1; } if (n == 1) { return (uint8_t)c; } return -1; } int16_t SITLUARTDriver::peek(void) { return -1; } void SITLUARTDriver::flush(void) { } size_t SITLUARTDriver::write(uint8_t c) { int flags = MSG_NOSIGNAL; _check_connection(); if (!_connected) { return 0; } if (_nonblocking_writes) { flags |= MSG_DONTWAIT; } if (_console) { return ::write(_fd, &c, 1); } return send(_fd, &c, 1, flags); } // BetterStream method implementations ///////////////////////////////////////// void SITLUARTDriver::print_P(const prog_char_t *s) { char c; while ('\0' != (c = pgm_read_byte((const prog_char *)s++))) write(c); } void SITLUARTDriver::println_P(const prog_char_t *s) { print_P(s); println(); } void SITLUARTDriver::printf(const char *fmt, ...) { va_list ap; va_start(ap, fmt); vprintf((AP_HAL::Print*)this, 0, fmt, ap); va_end(ap); } void SITLUARTDriver::_printf_P(const prog_char *fmt, ...) { va_list ap; va_start(ap, fmt); vprintf((AP_HAL::Print*)this, 1, fmt, ap); va_end(ap); } /* start a TCP connection for the serial port. If wait_for_connection is true then block until a client connects */ void SITLUARTDriver::_tcp_start_connection(bool wait_for_connection) { int one=1; struct sockaddr_in sockaddr; int ret; if (_console) { // hack for console access _connected = true; _listen_fd = -1; _fd = 1; _set_nonblocking(0); return; } memset(&sockaddr,0,sizeof(sockaddr)); #ifdef HAVE_SOCK_SIN_LEN sockaddr.sin_len = sizeof(sockaddr); #endif sockaddr.sin_port = htons(LISTEN_BASE_PORT + _portNumber); sockaddr.sin_family = AF_INET; _listen_fd = socket(AF_INET, SOCK_STREAM, 0); if (_listen_fd == -1) { fprintf(stderr, "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)); ret = bind(_listen_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); if (ret == -1) { fprintf(stderr, "bind failed on port %u - %s\n", (unsigned)ntohs(sockaddr.sin_port), strerror(errno)); exit(1); } ret = listen(_listen_fd, 5); if (ret == -1) { fprintf(stderr, "listen failed - %s\n", strerror(errno)); exit(1); } printf("Serial port %u on TCP port %u\n", _portNumber, LISTEN_BASE_PORT + _portNumber); fflush(stdout); if (wait_for_connection) { printf("Waiting for connection ....\n"); _fd = accept(_listen_fd, NULL, NULL); if (_fd == -1) { fprintf(stderr, "accept() error - %s", strerror(errno)); exit(1); } setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); _connected = true; _set_nonblocking(_fd); } } /* see if a new connection is coming in */ void SITLUARTDriver::_check_connection(void) { if (_connected) { // we only want 1 connection at a time return; } if (_select_check(_listen_fd)) { _fd = accept(_listen_fd, NULL, NULL); if (_fd != -1) { int one = 1; _connected = true; setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)); printf("New connection on serial port %u\n", _portNumber); _set_nonblocking(_fd); } } } /* use select() to see if something is pending */ bool SITLUARTDriver::_select_check(int fd) { fd_set fds; struct timeval tv; FD_ZERO(&fds); FD_SET(fd, &fds); // zero time means immediate return from select() tv.tv_sec = 0; tv.tv_usec = 0; if (select(fd+1, &fds, NULL, NULL, &tv) == 1) { return true; } return false; } void SITLUARTDriver::_set_nonblocking(int fd) { unsigned v = fcntl(fd, F_GETFL, 0); fcntl(fd, F_SETFL, v | O_NONBLOCK); } #endif