pios_tcp.c

Go to the documentation of this file.

/*
 * 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/>
 *
 * Additional note on redistribution: The copyright and license notices above
 * must be maintained in each individual source file that is a derivative work
 * of this source file; otherwise redistribution is prohibited.
 */


/* Project Includes */
#include "pios.h"

#if defined(PIOS_INCLUDE_TCP)

#include <pios_tcp_priv.h>
#include "pios_thread.h"
#include <unistd.h>
#include <sys/types.h>
#include <errno.h>
#include <fcntl.h>

#ifndef INVALID_SOCKET
#define INVALID_SOCKET (-1)
#endif

/* Provide a COM driver */
static void PIOS_TCP_ChangeBaud(uintptr_t tcp_id, uint32_t baud);
static void PIOS_TCP_RegisterRxCallback(uintptr_t tcp_id, pios_com_callback rx_in_cb, uintptr_t context);
static void PIOS_TCP_RegisterTxCallback(uintptr_t tcp_id, pios_com_callback tx_out_cb, uintptr_t context);
static void PIOS_TCP_TxStart(uintptr_t tcp_id, uint16_t tx_bytes_avail);
static void PIOS_TCP_RxStart(uintptr_t tcp_id, uint16_t rx_bytes_avail);
static bool PIOS_TCP_Available(uintptr_t tcp_id);

typedef struct {
        const struct pios_tcp_cfg * cfg;

        int socket;
        struct sockaddr_in6 server;
        struct sockaddr_in6 client;
        int socket_connection;

        pios_com_callback tx_out_cb;
        uintptr_t tx_out_context;
        pios_com_callback rx_in_cb;
        uintptr_t rx_in_context;

        uint8_t rx_buffer[PIOS_TCP_RX_BUFFER_SIZE];
        uint8_t tx_buffer[PIOS_TCP_RX_BUFFER_SIZE];
} pios_tcp_dev;

const struct pios_com_driver pios_tcp_com_driver = {
        .set_baud   = PIOS_TCP_ChangeBaud,
        .tx_start   = PIOS_TCP_TxStart,
        .rx_start   = PIOS_TCP_RxStart,
        .bind_tx_cb = PIOS_TCP_RegisterTxCallback,
        .bind_rx_cb = PIOS_TCP_RegisterRxCallback,
        .available  = PIOS_TCP_Available,
};


static pios_tcp_dev * find_tcp_dev_by_id(uintptr_t tcp)
{
        return (pios_tcp_dev *) tcp;
}

static void rx_cb_all(pios_tcp_dev *tcp_dev, uint8_t *incoming_buffer,
                int len) {
        int sent = 0;

        bool rx_need_yield = false;

        sent = tcp_dev->rx_in_cb(tcp_dev->rx_in_context, incoming_buffer, len,
                        NULL, &rx_need_yield);

        if (sent < 0) {
                return;
        }

        while (sent < len) {
                PIOS_Thread_Sleep(2);

                int sent_chunk = tcp_dev->rx_in_cb(tcp_dev->rx_in_context,
                        incoming_buffer, len, NULL, &rx_need_yield);

                if (sent_chunk < 0) {
                        return;
                }

                sent += sent_chunk;
        }
}

static void PIOS_TCP_RxTask(void *tcp_dev_n)
{
        pios_tcp_dev *tcp_dev = (pios_tcp_dev*)tcp_dev_n;
        
        const int INCOMING_BUFFER_SIZE = 16;
        uint8_t incoming_buffer[INCOMING_BUFFER_SIZE];
        int error;

        while (1) {
        
                do
                {
                        tcp_dev->socket_connection = accept(tcp_dev->socket, NULL, NULL);
                        error = errno;

                        PIOS_Thread_Sleep(1);
                } while (tcp_dev->socket_connection == INVALID_SOCKET && (error == EINTR || error == EAGAIN));

                if (tcp_dev->socket_connection < 0) {
                        int error = errno;
                        (void)error;
                        perror("Accept failed");
                        close(tcp_dev->socket);
                        exit(EXIT_FAILURE);
                }

                fprintf(stderr, "Connection accepted\n");

                while (1) {
                        // Received is used to track the scoket whereas the dev variable is only updated when it can be

                        int result = recv(tcp_dev->socket_connection,
                                        (void *) incoming_buffer,
                                        INCOMING_BUFFER_SIZE, 0);
                        error = errno;

                        if (result > 0 && tcp_dev->rx_in_cb) {
                                /* While on other drivers it may be desirable to
                                 * spill immediately if the consumer is not
                                 * keeping up, TCP is self-regulating in speed
                                 * and GCS may want to really hammer us.  Let's
                                 * not let the client run-ahead and force us to
                                 * drop stuff that we've read.
                                 */

                                rx_cb_all(tcp_dev, incoming_buffer, result);

                        }

                        if (result == 0) {
                                break;
                        }

                        if (result == -1) {
                                if (error == EAGAIN)
                                        PIOS_Thread_Sleep(1);
                                else if (error == EINTR)
                                        (void)error;
                                else
                                        break;
                        }
                }
                
                close(tcp_dev->socket_connection);
                tcp_dev->socket_connection = INVALID_SOCKET;
        }
}


struct pios_thread *tcpRxTaskHandle;
int32_t PIOS_TCP_Init(uintptr_t *tcp_id, const struct pios_tcp_cfg * cfg)
{
        pios_tcp_dev *tcp_dev = PIOS_malloc(sizeof(pios_tcp_dev));

        memset(tcp_dev, 0, sizeof(*tcp_dev));

        /* initialize */
        tcp_dev->rx_in_cb = NULL;
        tcp_dev->tx_out_cb = NULL;
        tcp_dev->cfg=cfg;
        
        /* assign socket */
        tcp_dev->socket = socket(PF_INET6, SOCK_STREAM, IPPROTO_TCP);
        tcp_dev->socket_connection = INVALID_SOCKET;

#if defined(_WIN32) || defined(WIN32) || defined(__MINGW32__)
        char optval = 1;
        char optoff = 0;

        setsockopt(tcp_dev->socket, IPPROTO_IPV6, IPV6_V6ONLY, &optoff, sizeof(optoff));
#else
        int optval = 1;
#endif

        /* Allow reuse of address if you restart. */
        setsockopt(tcp_dev->socket, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval));

        /* Also request low-latency (don't store up data to conserve packets */
        setsockopt(tcp_dev->socket, IPPROTO_TCP, TCP_NODELAY, &optval, sizeof(optval));

        memset(&tcp_dev->server, 0, sizeof(tcp_dev->server));
        memset(&tcp_dev->client, 0, sizeof(tcp_dev->client));

        tcp_dev->server.sin6_family = AF_INET6;
        tcp_dev->server.sin6_addr = in6addr_any;
        tcp_dev->server.sin6_port = htons(tcp_dev->cfg->port);

        int res = bind(tcp_dev->socket, (struct sockaddr*)&tcp_dev->server, sizeof(tcp_dev->server));
        if (res == -1) {
                perror("Binding socket failed");
                exit(EXIT_FAILURE);
        }
        
        res = listen(tcp_dev->socket, 10);
        if (res == -1) {
                perror("Socket listen failed");
                exit(EXIT_FAILURE);
        }
        
        tcpRxTaskHandle = PIOS_Thread_Create(
                        PIOS_TCP_RxTask, "pios_tcp_rx", PIOS_THREAD_STACK_SIZE_MIN, tcp_dev, PIOS_THREAD_PRIO_HIGHEST);
        
        printf("tcp dev %p - socket %i opened - result %i\n", tcp_dev, tcp_dev->socket, res);
        
        *tcp_id = (uintptr_t) tcp_dev;
        
        return res;
}


void PIOS_TCP_ChangeBaud(uintptr_t tcp_id, uint32_t baud)
{
}


static void PIOS_TCP_RxStart(uintptr_t tp_id, uint16_t rx_bytes_avail)
{
}


static void PIOS_TCP_TxStart(uintptr_t tcp_id, uint16_t tx_bytes_avail)
{
        pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id);
        
        PIOS_Assert(tcp_dev);
        
        int32_t length,rem;
        
        if (tcp_dev->tx_out_cb) {
                while (tx_bytes_avail > 0) {
                        bool tx_need_yield = false;
                        length = (tcp_dev->tx_out_cb)(tcp_dev->tx_out_context, tcp_dev->tx_buffer, PIOS_TCP_RX_BUFFER_SIZE, NULL, &tx_need_yield);
                        rem = length;
                        while (rem > 0) {
                                ssize_t len = 0;
                                if (tcp_dev->socket_connection != INVALID_SOCKET) {
                                        len = send(tcp_dev->socket_connection, (char *) tcp_dev->tx_buffer, length, 0);
                                }
                                if (len <= 0) {
                                        rem = 0;
                                } else {
                                        rem -= len;
                                }
                        }
                        tx_bytes_avail -= length;
                }
        }
}

static void PIOS_TCP_RegisterRxCallback(uintptr_t tcp_id, pios_com_callback rx_in_cb, uintptr_t context)
{
        pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id);
        
        PIOS_Assert(tcp_dev);
        
        /*
         * Order is important in these assignments since ISR uses _cb
         * field to determine if it's ok to dereference _cb and _context
         */
        tcp_dev->rx_in_context = context;
        tcp_dev->rx_in_cb = rx_in_cb;
}

static void PIOS_TCP_RegisterTxCallback(uintptr_t tcp_id, pios_com_callback tx_out_cb, uintptr_t context)
{
        pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id);
        
        PIOS_Assert(tcp_dev);
        
        /*
         * Order is important in these assignments since ISR uses _cb
         * field to determine if it's ok to dereference _cb and _context
         */
        tcp_dev->tx_out_context = context;
        tcp_dev->tx_out_cb = tx_out_cb;
}


static bool PIOS_TCP_Available(uintptr_t tcp_id)
{
        pios_tcp_dev *tcp_dev = find_tcp_dev_by_id(tcp_id);

        PIOS_Assert(tcp_dev);

        return tcp_dev->socket_connection != INVALID_SOCKET;
}

#endif