src/main/drivers/serial_tcp.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software. You can redistribute this software
   5  * and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * INAV is distributed in the hope that they will be
  11  * useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26 #include <sys/select.h>
  27 #include <time.h>
  28 #include <unistd.h>
  29
  30 #include "platform.h"
  31
  32 #if defined(SITL_BUILD)
  33
  34 #include <sys/socket.h>
  35 #include <fcntl.h>
  36 #include <netinet/in.h>
  37 #include <netdb.h>
  38 #include <arpa/inet.h>
  39 #include <errno.h>
  40 #include <netinet/tcp.h>
  41
  42 #include "common/utils.h"
  43
  44 #include "drivers/serial.h"
  45 #include "drivers/serial_tcp.h"
  46 #include "target/SITL/serial_proxy.h"
  47
  48 static const struct serialPortVTable tcpVTable[];
  49 static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
  50
  51 static void *tcpReceiveThread(void* arg)
  52 {
  53     tcpPort_t *port = (tcpPort_t*)arg;
  54     while(tcpReceive(port) >= 0)
  55         ;
  56     return NULL;
  57 }
  58 static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
  59 {
  60     socklen_t sockaddrlen;
  61     if (port->isInitalized){
  62         return port;
  63     }
  64
  65     if (pthread_mutex_init(&port->receiveMutex, NULL) != 0){
  66         return NULL;
  67     }
  68
  69     uint16_t tcpPort = BASE_IP_ADDRESS + id - 1;
  70     if (lookupAddress(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) {
  71             return NULL;
  72     }
  73     port->socketFd = socket(((struct sockaddr*)&port->sockAddress)->sa_family, SOCK_STREAM, IPPROTO_TCP);
  74
  75     if (port->socketFd < 0) {
  76         fprintf(stderr, "[SOCKET] Unable to create tcp socket\n");
  77         return NULL;
  78     }
  79     int err = 0;
  80 #ifdef __CYGWIN__
  81     // Sadly necesary to enforce dual-stack behaviour on Windows networking ,,,
  82     if (((struct sockaddr*)&port->sockAddress)->sa_family == AF_INET6) {
  83         int v6only=0;
  84         err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only));
  85         if (err != 0) {
  86             fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno));
  87         }
  88     }
  89 #endif
  90
  91     int one = 1;
  92     err = setsockopt(port->socketFd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one));
  93     err = setsockopt(port->socketFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
  94     err = fcntl(port->socketFd, F_SETFL, fcntl(port->socketFd, F_GETFL, 0) | O_NONBLOCK);
  95
  96     if (err < 0){
  97         fprintf(stderr, "[SOCKET] Unable to set socket options\n");
  98         return NULL;
  99     }
 100
 101     port->isClientConnected = false;
 102     port->isInitalized = true;
 103     port->id = id;
 104
 105     if (bind(port->socketFd, (struct sockaddr*)&port->sockAddress, sockaddrlen) < 0) {
 106         fprintf(stderr, "[SOCKET] Unable to bind socket\n");
 107         return NULL;
 108     }
 109
 110     if (listen(port->socketFd, 100) < 0) {
 111         fprintf(stderr, "[SOCKET] Unable to listen.\n");
 112         return NULL;
 113     }
 114     char addrbuf[IPADDRESS_PRINT_BUFLEN];
 115     char *addrptr = prettyPrintAddress((struct sockaddr *)&port->sockAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
 116     if (addrptr != NULL) {
 117         fprintf(stderr, "[SOCKET] Bind TCP %s to UART%d\n", addrptr, id);
 118     }
 119     return port;
 120 }
 121
 122 void tcpReceiveBytes( tcpPort_t *port, const uint8_t* buffer, ssize_t recvSize ) {
 123     for (ssize_t i = 0; i < recvSize; i++) {
 124         if (port->serialPort.rxCallback) {
 125             port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
 126         } else {
 127             pthread_mutex_lock(&port->receiveMutex);
 128             port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
 129             port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
 130             pthread_mutex_unlock(&port->receiveMutex);
 131         }
 132     }
 133 }
 134
 135 void tcpReceiveBytesEx( int portIndex, const uint8_t* buffer, ssize_t recvSize ) {
 136     tcpReceiveBytes( &tcpPorts[portIndex], buffer, recvSize );
 137 }
 138
 139 int tcpReceive(tcpPort_t *port)
 140 {
 141     char addrbuf[IPADDRESS_PRINT_BUFLEN];
 142     if (!port->isClientConnected) {
 143
 144         fd_set fds;
 145
 146         FD_ZERO(&fds);
 147         FD_SET(port->socketFd, &fds);
 148
 149         if (select(port->socketFd + 1, &fds, NULL, NULL, NULL) < 0) {
 150             fprintf(stderr, "[SOCKET] Unable to wait for connection.\n");
 151             return -1;
 152         }
 153
 154         socklen_t addrLen = sizeof(struct sockaddr_storage);
 155         port->clientSocketFd = accept(port->socketFd,(struct sockaddr*)&port->clientAddress, &addrLen);
 156         if (port->clientSocketFd < 1) {
 157             fprintf(stderr, "[SOCKET] Can't accept connection.\n");
 158             return -1;
 159         }
 160
 161         char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
 162         if (addrptr != NULL) {
 163            fprintf(stderr, "[SOCKET] %s connected to UART%d\n", addrptr, port->id);
 164         }
 165         port->isClientConnected = true;
 166     }
 167
 168     uint8_t buffer[TCP_BUFFER_SIZE];
 169     ssize_t recvSize = recv(port->clientSocketFd, buffer, TCP_BUFFER_SIZE, 0);
 170
 171     // recv() under cygwin does not recognise the closed connection under certain circumstances, but returns ECONNRESET as an error.
 172     if (port->isClientConnected && (recvSize == 0 || ( recvSize == -1 && errno == ECONNRESET))) {
 173         char *addrptr = prettyPrintAddress((struct sockaddr *)&port->clientAddress, addrbuf, IPADDRESS_PRINT_BUFLEN);
 174         if (addrptr != NULL) {
 175             fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", addrptr, port->id);
 176         }
 177         close(port->clientSocketFd);
 178         memset(&port->clientAddress, 0, sizeof(port->clientAddress));
 179         port->isClientConnected = false;
 180         return 0;
 181     }
 182
 183     if (recvSize < 0) {
 184         recvSize = 0;
 185     }
 186
 187     tcpReceiveBytes( port, buffer, recvSize );
 188
 189     return (int)recvSize;
 190 }
 191
 192 serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options)
 193 {
 194     tcpPort_t *port = NULL;
 195
 196 #if defined(USE_UART1) || defined(USE_UART2) || defined(USE_UART3) || defined(USE_UART4) || defined(USE_UART5) || defined(USE_UART6) || defined(USE_UART7) || defined(USE_UART8)
 197     uint32_t id = (uintptr_t)USARTx;
 198     if (id <= SERIAL_PORT_COUNT) {
 199         port = tcpReConfigure(&tcpPorts[id-1], id);
 200     }
 201 #endif
 202
 203     if (port == NULL) {
 204          return NULL;
 205
 206     }
 207
 208     port->serialPort.vTable = tcpVTable;
 209     port->serialPort.rxCallback = callback;
 210     port->serialPort.rxCallbackData = rxCallbackData;
 211     port->serialPort.rxBufferHead = port->serialPort.rxBufferTail = 0;
 212     port->serialPort.rxBufferSize = TCP_BUFFER_SIZE;
 213     port->serialPort.rxBuffer = port->rxBuffer;
 214     port->serialPort.mode = mode;
 215     port->serialPort.baudRate = baudRate;
 216     port->serialPort.options = options;
 217
 218     int err = pthread_create(&port->receiveThread, NULL, tcpReceiveThread, (void*)port);
 219     if (err < 0){
 220         fprintf(stderr, "[SOCKET] Unable to create receive thread for UART%d\n", id);
 221         return NULL;
 222     }
 223     return (serialPort_t*)port;
 224 }
 225
 226 uint8_t tcpRead(serialPort_t *instance)
 227 {
 228     uint8_t ch;
 229     tcpPort_t *port = (tcpPort_t*)instance;
 230     pthread_mutex_lock(&port->receiveMutex);
 231
 232     ch = port->serialPort.rxBuffer[port->serialPort.rxBufferTail];
 233     port->serialPort.rxBufferTail = (port->serialPort.rxBufferTail + 1) % port->serialPort.rxBufferSize;
 234
 235     pthread_mutex_unlock(&port->receiveMutex);
 236
 237     return ch;
 238 }
 239
 240 void tcpWritBuf(serialPort_t *instance, const void *data, int count)
 241 {
 242     tcpPort_t *port = (tcpPort_t*)instance;
 243
 244     if (!port->isClientConnected) {
 245         return;
 246     }
 247
 248     send(port->clientSocketFd, data, count, 0);
 249 }
 250
 251 int getTcpPortIndex(const serialPort_t *instance) {
 252     for (int i = 0; i < SERIAL_PORT_COUNT; i++) {
 253         if ( &(tcpPorts[i].serialPort) == instance) return i;
 254     }
 255     return -1;
 256 }
 257
 258 void tcpWrite(serialPort_t *instance, uint8_t ch)
 259 {
 260     tcpWritBuf(instance, (void*)&ch, 1);
 261
 262     int index = getTcpPortIndex(instance);
 263     if ( !serialFCProxy && serialProxyIsConnected() && (index == (serialUartIndex-1)) ) {
 264             serialProxyWriteData( (unsigned char *)&ch, 1);
 265     }
 266 }
 267
 268 uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
 269 {
 270     tcpPort_t *port = (tcpPort_t*)instance;
 271     uint32_t count;
 272
 273     pthread_mutex_lock(&port->receiveMutex);
 274
 275     if (port->serialPort.rxBufferHead >= port->serialPort.rxBufferTail) {
 276         count = port->serialPort.rxBufferHead - port->serialPort.rxBufferTail;
 277     } else {
 278         count = port->serialPort.rxBufferSize + port->serialPort.rxBufferHead - port->serialPort.rxBufferTail;
 279     }
 280
 281     pthread_mutex_unlock(&port->receiveMutex);
 282
 283     return count;
 284 }
 285
 286 uint32_t tcpRXBytesFree(int portIndex) {
 287     return tcpPorts[portIndex].serialPort.rxBufferSize - tcpTotalRxBytesWaiting( &tcpPorts[portIndex].serialPort);
 288 }
 289
 290 uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
 291 {
 292     UNUSED(instance);
 293     return TCP_MAX_PACKET_SIZE;
 294 }
 295
 296 bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
 297 {
 298     UNUSED(instance);
 299     return true;
 300 }
 301
 302 bool tcpIsConnected(const serialPort_t *instance)
 303 {
 304     return ((tcpPort_t*)instance)->isClientConnected;
 305 }
 306
 307 void tcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
 308 {
 309     UNUSED(instance);
 310     UNUSED(baudRate);
 311 }
 312
 313
 314 void tcpSetMode(serialPort_t *instance, portMode_t mode)
 315 {
 316     UNUSED(instance);
 317     UNUSED(mode);
 318 }
 319
 320 static const struct serialPortVTable tcpVTable[] = {
 321     {
 322         .serialWrite = tcpWrite,
 323         .serialTotalRxWaiting = tcpTotalRxBytesWaiting,
 324         .serialTotalTxFree = tcpTotalTxBytesFree,
 325         .serialRead = tcpRead,
 326         .serialSetBaudRate = tcpSetBaudRate,
 327         .isSerialTransmitBufferEmpty = isTcpTransmitBufferEmpty,
 328         .setMode = tcpSetMode,
 329         .isConnected = tcpIsConnected,
 330         .writeBuf = tcpWritBuf,
 331         .beginWrite = NULL,
 332         .endWrite = NULL,
 333         .isIdle = NULL,
 334     }
 335 };
 336
 337 #endif