zr-v4/libraries/AP_HAL_SITL/UARTDriver.cpp

// -*- Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
   This program is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

//
//      Copyright (c) 2010 Michael Smith. All rights reserved.
//
#include <AP_HAL/AP_HAL.h>
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL

#include <limits.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <stdarg.h>
#include <AP_Math/AP_Math.h>

#include <errno.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/select.h>
#include <termios.h>

#include "UARTDriver.h"
#include "SITL_State.h"

extern const AP_HAL::HAL& hal;

using namespace HALSITL;

bool SITLUARTDriver::_console;

/* UARTDriver method implementations */

void SITLUARTDriver::begin(uint32_t baud, uint16_t rxSpace, uint16_t txSpace)
{
    const char *path = _sitlState->_uart_path[_portNumber];

    if (strcmp(path, "GPS1") == 0) {
        /* gps */
        _connected = true;
        _fd = _sitlState->gps_pipe();
    } else if (strcmp(path, "GPS2") == 0) {
        /* 2nd gps */
        _connected = true;
        _fd = _sitlState->gps2_pipe();
    } else {
        /* parse type:args:flags string for path. 
           For example:
             tcp:5760:wait    // tcp listen on port 5760
             tcp:0:wait       // tcp listen on use base_port + 0
             tcpclient:192.168.2.15:5762
             uart:/dev/ttyUSB0:57600
         */
        char *saveptr = NULL;
        char *s = strdup(path);
        char *devtype = strtok_r(s, ":", &saveptr);
        char *args1 = strtok_r(NULL, ":", &saveptr);
        char *args2 = strtok_r(NULL, ":", &saveptr);
        if (strcmp(devtype, "tcp") == 0) {
            uint16_t port = atoi(args1);
            bool wait = (args2 && strcmp(args2, "wait") == 0);
            _tcp_start_connection(port, wait);
        } else if (strcmp(devtype, "tcpclient") == 0) {
            if (args2 == NULL) {
                AP_HAL::panic("Invalid tcp client path: %s", path);
            }
            uint16_t port = atoi(args2);
            _tcp_start_client(args1, port);
        } else if (strcmp(devtype, "uart") == 0) {
            uint32_t baudrate = args2? atoi(args2) : 57600;
            ::printf("UART connection %s:%u\n", args1, baudrate);
            _uart_start_connection(args1, baudrate);
        } else {
            AP_HAL::panic("Invalid device path: %s", path);
        }
        free(s);
    }

    _set_nonblocking(_fd);
}

void SITLUARTDriver::end()
{
}

int16_t SITLUARTDriver::available(void)
{
    _check_connection();

    if (!_connected) {
        return 0;
    }

    return _readbuffer.available();
}



int16_t SITLUARTDriver::txspace(void)
{
    _check_connection();
    if (!_connected) {
        return 0;
    }
    return _writebuffer.space();
}

int16_t SITLUARTDriver::read(void)
{
    if (available() <= 0) {
        return -1;
    }
    uint8_t c;
    _readbuffer.read(&c, 1);
    return c;
}

void SITLUARTDriver::flush(void)
{
}

size_t SITLUARTDriver::write(uint8_t c)
{
    if (txspace() <= 0) {
        return 0;
    }
    _writebuffer.write(&c, 1);
    return 1;
}

size_t SITLUARTDriver::write(const uint8_t *buffer, size_t size)
{
    if (txspace() <= (ssize_t)size) {
        size = txspace();
    }
    if (size <= 0) {
        return 0;
    }
    _writebuffer.write(buffer, size);
    return size;
}

    
/*
  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(uint16_t port, bool wait_for_connection)
{
    int one=1;
    struct sockaddr_in sockaddr;
    int ret;

    if (_connected) {
        return;
    }

    _use_send_recv = true;

    if (_console) {
        // hack for console access
        _connected = true;
        _use_send_recv = false;
        _listen_fd = -1;
        _fd = 1;
        return;
    }

    if (_fd != -1) {
        close(_fd);
    }

    if (_listen_fd == -1) {
        memset(&sockaddr,0,sizeof(sockaddr));

#ifdef HAVE_SOCK_SIN_LEN
        sockaddr.sin_len = sizeof(sockaddr);
#endif
        if (port > 1000) {
            sockaddr.sin_port = htons(port);
        } else {
            sockaddr.sin_port = htons(_sitlState->base_port() + port);
        }
        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));

        fprintf(stderr, "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) {
            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);
        }

        fprintf(stderr, "Serial port %u on TCP port %u\n", _portNumber,
                _sitlState->base_port() + _portNumber);
        fflush(stdout);
    }

    if (wait_for_connection) {
        fprintf(stdout, "Waiting for connection ....\n");
        fflush(stdout);
        _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;
    }
}


/*
  start a TCP client connection for the serial port. 
 */
void SITLUARTDriver::_tcp_start_client(const char *address, uint16_t port)
{
    int one=1;
    struct sockaddr_in sockaddr;
    int ret;

    if (_connected) {
        return;
    }

    _use_send_recv = true;
    
    if (_fd != -1) {
        close(_fd);
    }

    memset(&sockaddr,0,sizeof(sockaddr));

#ifdef HAVE_SOCK_SIN_LEN
    sockaddr.sin_len = sizeof(sockaddr);
#endif
    sockaddr.sin_port = port;
    sockaddr.sin_family = AF_INET;
    sockaddr.sin_addr.s_addr = inet_addr(address);

    _fd = socket(AF_INET, SOCK_STREAM, 0);
    if (_fd == -1) {
        fprintf(stderr, "socket failed - %s\n", strerror(errno));
        exit(1);
    }

    /* we want to be able to re-use ports quickly */
    setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));

    ret = connect(_fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr));
    if (ret == -1) {
        fprintf(stderr, "connect failed on port %u - %s\n",
                (unsigned)ntohs(sockaddr.sin_port),
                strerror(errno));
        exit(1);
    }

    setsockopt(_fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
    setsockopt(_fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
    _connected = true;
}


/*
  start a UART connection for the serial port
 */
void SITLUARTDriver::_uart_start_connection(const char *path, uint32_t baudrate)
{
    struct termios t {};
    if (!_connected) {
        ::printf("Opening %s\n", path);
        _fd = ::open(path, O_RDWR | O_CLOEXEC);
    }

    if (_fd == -1) {
        AP_HAL::panic("Unable to open UART %s", path);
    }

    // set non-blocking
    int flags = fcntl(_fd, F_GETFL, 0);
    flags = flags | O_NONBLOCK;
    fcntl(_fd, F_SETFL, flags);

    // disable LF -> CR/LF
    tcgetattr(_fd, &t);
    t.c_iflag &= ~(BRKINT | ICRNL | IMAXBEL | IXON | IXOFF);
    t.c_oflag &= ~(OPOST | ONLCR);
    t.c_lflag &= ~(ISIG | ICANON | IEXTEN | ECHO | ECHOE | ECHOK | ECHOCTL | ECHOKE);
    t.c_cc[VMIN] = 0;
    tcsetattr(_fd, TCSANOW, &t);

    // set baudrate
    tcgetattr(_fd, &t);
    cfsetspeed(&t, baudrate);
    tcsetattr(_fd, TCSANOW, &t);

    _connected = true;
    _use_send_recv = false;
}

/*
  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));
            fprintf(stdout, "New connection on serial port %u\n", _portNumber);
        }
    }
}

/*
  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);
}

void SITLUARTDriver::_timer_tick(void)
{
    uint32_t navail;
    ssize_t nwritten;

    const uint8_t *readptr = _writebuffer.readptr(navail);
    if (readptr && navail > 0) {
        if (!_use_send_recv) {
            nwritten = ::write(_fd, readptr, navail);
        } else {
            nwritten = send(_fd, readptr, navail, MSG_DONTWAIT);
        }
        if (nwritten > 0) {
            _writebuffer.advance(nwritten);
        }
    }

    if (_readbuffer.space() == 0) {
        return;
    }
    
    ssize_t nread;
    char c;
    if (!_use_send_recv) {
        int fd = _console?0:_fd;
        nread = ::read(fd, &c, 1);
    } else {
        if (_select_check(_fd)) {
            nread = recv(_fd, &c, 1, MSG_DONTWAIT);
            if (nread <= 0) {
                // the socket has reached EOF
                close(_fd);
                _connected = false;
                fprintf(stdout, "Closed connection on serial port %u\n", _portNumber);
                fflush(stdout);
                return;
            }
        } else {
            nread = 0;
        }
    }
    if (nread == 1) {
        _readbuffer.write((uint8_t *)&c, 1);
    }
}

#endif // CONFIG_HAL_BOARD