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
  41 #include "common/utils.h"
  42
  43 #include "drivers/serial.h"
  44 #include "drivers/serial_tcp.h"
  45
  46 static const struct serialPortVTable tcpVTable[];
  47 static tcpPort_t tcpPorts[SERIAL_PORT_COUNT];
  48 static bool tcpThreadRunning = false;
  49
  50 static int lookup_address (char *name, int port, int type, struct sockaddr *addr, socklen_t* len )
  51 {
  52     struct addrinfo *servinfo, *p;
  53     struct addrinfo hints = {.ai_family = AF_UNSPEC, .ai_socktype = type, .ai_flags = AI_V4MAPPED|AI_ADDRCONFIG};
  54     if (name == NULL) {
  55         hints.ai_flags |= AI_PASSIVE;
  56     }
  57     /*
  58       This nonsense is to uniformly deliver the same sa_family regardless of whether
  59       name is NULL or non-NULL
  60       Otherwise, at least on Linux, we get
  61       - V6,V4 for the non-null case and
  62       - V4,V6 for the null case, regardless of gai.conf
  63       Which may confuse consumers.
  64       FreeBSD and Windows behave consistently, giving V6 for Ipv6 enabled stacks in all cases
  65       unless a quad dotted address is specificied
  66     */
  67     struct addrinfo *p4 = NULL;
  68     struct addrinfo *p6 = NULL;
  69     int result;
  70     char aport[16];
  71     snprintf(aport, sizeof(aport), "%d", port);
  72     if ((result = getaddrinfo(name, aport, &hints, &servinfo)) != 0) {
  73         fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(result));
  74         return result;
  75     } else {
  76         int j = 0;
  77         for(p = servinfo; p != NULL; p = p->ai_next) {
  78             if(p->ai_family == AF_INET6)
  79                 p6 = p;
  80             else if(p->ai_family == AF_INET)
  81         p4 = p;
  82             j++;
  83         }
  84         if (p6 != NULL)
  85             p = p6;
  86         else if (p4 != NULL)
  87             p = p4;
  88         else
  89             return -1;
  90         memcpy(addr, p->ai_addr, p->ai_addrlen);
  91         *len = p->ai_addrlen;
  92         freeaddrinfo(servinfo);
  93     }
  94     return 0;
  95 }
  96
  97 static char *tcpGetAddressString(struct sockaddr *addr)
  98 {
  99     static char straddr[INET6_ADDRSTRLEN];
 100     void *ipaddr;
 101     if (addr->sa_family == AF_INET6) {
 102         struct sockaddr_in6 * ip = (struct sockaddr_in6*)addr;
 103         ipaddr = &ip->sin6_addr;
 104     } else {
 105         struct sockaddr_in * ip = (struct sockaddr_in*)addr;
 106         ipaddr = &ip->sin_addr;
 107     }
 108     const char *res = inet_ntop(addr->sa_family, ipaddr, straddr, sizeof straddr);
 109     return (char *)res;
 110 }
 111
 112 static void *tcpReceiveThread(void* arg)
 113 {
 114     tcpPort_t *port = (tcpPort_t*)arg;
 115
 116     while(tcpThreadRunning) {
 117         if (tcpReceive(port) < 0) {
 118             break;
 119         }
 120     }
 121
 122     return NULL;
 123 }
 124
 125 static tcpPort_t *tcpReConfigure(tcpPort_t *port, uint32_t id)
 126 {
 127     socklen_t sockaddrlen;
 128     if (port->isInitalized){
 129         return port;
 130     }
 131
 132     if (pthread_mutex_init(&port->receiveMutex, NULL) != 0){
 133         return NULL;
 134     }
 135
 136     uint16_t tcpPort = BASE_IP_ADDRESS + id - 1;
 137     if (lookup_address(NULL, tcpPort, SOCK_STREAM, (struct sockaddr*)&port->sockAddress, &sockaddrlen) != 0) {
 138         return NULL;
 139     }
 140     port->socketFd = socket(((struct sockaddr*)&port->sockAddress)->sa_family, SOCK_STREAM, IPPROTO_TCP);
 141
 142     if (port->socketFd < 0) {
 143         fprintf(stderr, "[SOCKET] Unable to create tcp socket\n");
 144         return NULL;
 145     }
 146     int err = 0;
 147 #ifdef __CYGWIN__
 148     // Sadly necesary to enforce dual-stack behaviour on Windows networking ,,,
 149     if (((struct sockaddr*)&port->sockAddress)->sa_family == AF_INET6) {
 150         int v6only=0;
 151         err = setsockopt(port->socketFd, IPPROTO_IPV6, IPV6_V6ONLY, &v6only, sizeof(v6only));
 152         if (err != 0) {
 153             fprintf(stderr,"[SOCKET] setting V6ONLY=false: %s\n", strerror(errno));
 154         }
 155     }
 156 #endif
 157
 158     int one = 1;
 159     err = setsockopt(port->socketFd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one));
 160     err = fcntl(port->socketFd, F_SETFL, fcntl(port->socketFd, F_GETFL, 0) | O_NONBLOCK);
 161
 162     if (err < 0){
 163         fprintf(stderr, "[SOCKET] Unable to set socket options\n");
 164         return NULL;
 165     }
 166
 167     port->isClientConnected = false;
 168     port->isInitalized = true;
 169     port->id = id;
 170
 171     if (bind(port->socketFd, (struct sockaddr*)&port->sockAddress, sockaddrlen) < 0) {
 172         fprintf(stderr, "[SOCKET] Unable to bind socket\n");
 173         return NULL;
 174     }
 175
 176     if (listen(port->socketFd, 100) < 0) {
 177         fprintf(stderr, "[SOCKET] Unable to listen.\n");
 178         return NULL;
 179     }
 180
 181     fprintf(stderr, "[SOCKET] Bind TCP %s port %d to UART%d\n",
 182             tcpGetAddressString((struct sockaddr*)&port->sockAddress), tcpPort, id);
 183
 184     return port;
 185 }
 186
 187 int tcpReceive(tcpPort_t *port)
 188 {
 189     if (!port->isClientConnected) {
 190
 191         fd_set fds;
 192
 193         FD_ZERO(&fds);
 194         FD_SET(port->socketFd, &fds);
 195
 196         if (select(port->socketFd + 1, &fds, NULL, NULL, NULL) < 0) {
 197             fprintf(stderr, "[SOCKET] Unable to wait for connection.\n");
 198             return -1;
 199         }
 200
 201         socklen_t addrLen = sizeof(struct sockaddr_storage);
 202         port->clientSocketFd = accept(port->socketFd,(struct sockaddr*)&port->clientAddress, &addrLen);
 203         if (port->clientSocketFd < 1) {
 204             fprintf(stderr, "[SOCKET] Can't accept connection.\n");
 205             return -1;
 206         }
 207
 208        fprintf(stderr, "[SOCKET] %s connected to UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
 209         port->isClientConnected = true;
 210     }
 211
 212     uint8_t buffer[TCP_BUFFER_SIZE];
 213     ssize_t recvSize = recv(port->clientSocketFd, buffer, TCP_BUFFER_SIZE, 0);
 214
 215     // Disconnect
 216     if (port->isClientConnected && recvSize == 0)
 217     {
 218        fprintf(stderr, "[SOCKET] %s disconnected from UART%d\n", tcpGetAddressString((struct sockaddr *)&port->clientAddress), port->id);
 219        close(port->clientSocketFd);
 220        memset(&port->clientAddress, 0, sizeof(port->clientAddress));
 221        port->isClientConnected = false;
 222
 223        return 0;
 224     }
 225
 226     for (ssize_t i = 0; i < recvSize; i++) {
 227
 228         if (port->serialPort.rxCallback) {
 229             port->serialPort.rxCallback((uint16_t)buffer[i], port->serialPort.rxCallbackData);
 230         } else {
 231             pthread_mutex_lock(&port->receiveMutex);
 232             port->serialPort.rxBuffer[port->serialPort.rxBufferHead] = buffer[i];
 233             port->serialPort.rxBufferHead = (port->serialPort.rxBufferHead + 1) % port->serialPort.rxBufferSize;
 234             pthread_mutex_unlock(&port->receiveMutex);
 235         }
 236     }
 237
 238     if (recvSize < 0) {
 239         recvSize = 0;
 240     }
 241
 242     return (int)recvSize;
 243 }
 244
 245 serialPort_t *tcpOpen(USART_TypeDef *USARTx, serialReceiveCallbackPtr callback, void *rxCallbackData, uint32_t baudRate, portMode_t mode, portOptions_t options)
 246 {
 247     tcpPort_t *port = NULL;
 248
 249 #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)
 250     uint32_t id = (uintptr_t)USARTx;
 251     if (id < SERIAL_PORT_COUNT) {
 252         port = tcpReConfigure(&tcpPorts[id-1], id);
 253     }
 254 #endif
 255
 256     if (port == NULL) {
 257          return NULL;
 258
 259     }
 260
 261     port->serialPort.vTable = tcpVTable;
 262     port->serialPort.rxCallback = callback;
 263     port->serialPort.rxCallbackData = rxCallbackData;
 264     port->serialPort.rxBufferHead = port->serialPort.rxBufferTail = 0;
 265     port->serialPort.rxBufferSize = TCP_BUFFER_SIZE;
 266     port->serialPort.rxBuffer = port->rxBuffer;
 267     port->serialPort.mode = mode;
 268     port->serialPort.baudRate = baudRate;
 269     port->serialPort.options = options;
 270
 271     int err = pthread_create(&port->receiveThread, NULL, tcpReceiveThread, (void*)port);
 272     if (err < 0){
 273         fprintf(stderr, "[SOCKET] Unable to create receive thread for UART%d\n", id);
 274         return NULL;
 275     }
 276     tcpThreadRunning = true;
 277
 278     return (serialPort_t*)port;
 279 }
 280
 281 uint8_t tcpRead(serialPort_t *instance)
 282 {
 283     uint8_t ch;
 284     tcpPort_t *port = (tcpPort_t*)instance;
 285     pthread_mutex_lock(&port->receiveMutex);
 286
 287     ch = port->serialPort.rxBuffer[port->serialPort.rxBufferTail];
 288     port->serialPort.rxBufferTail = (port->serialPort.rxBufferTail + 1) % port->serialPort.rxBufferSize;
 289
 290     pthread_mutex_unlock(&port->receiveMutex);
 291
 292     return ch;
 293 }
 294
 295 void tcpWritBuf(serialPort_t *instance, const void *data, int count)
 296 {
 297     tcpPort_t *port = (tcpPort_t*)instance;
 298
 299     if (!port->isClientConnected) {
 300         return;
 301     }
 302
 303     send(port->clientSocketFd, data, count, 0);
 304 }
 305
 306 void tcpWrite(serialPort_t *instance, uint8_t ch)
 307 {
 308     tcpWritBuf(instance, (void*)&ch, 1);
 309 }
 310
 311 uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
 312 {
 313     tcpPort_t *port = (tcpPort_t*)instance;
 314     uint32_t count;
 315
 316     pthread_mutex_lock(&port->receiveMutex);
 317
 318     if (port->serialPort.rxBufferHead >= port->serialPort.rxBufferTail) {
 319         count = port->serialPort.rxBufferHead - port->serialPort.rxBufferTail;
 320     } else {
 321         count = port->serialPort.rxBufferSize + port->serialPort.rxBufferHead - port->serialPort.rxBufferTail;
 322     }
 323
 324     pthread_mutex_unlock(&port->receiveMutex);
 325
 326     return count;
 327 }
 328
 329 uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
 330 {
 331     tcpPort_t *port = (tcpPort_t*)instance;
 332
 333     if (port->isClientConnected) {
 334         return TCP_MAX_PACKET_SIZE;
 335     } else {
 336         return 0;
 337     }
 338 }
 339
 340 bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
 341 {
 342     UNUSED(instance);
 343
 344     return true;
 345 }
 346
 347 bool tcpIsConnected(const serialPort_t *instance)
 348 {
 349     return ((tcpPort_t*)instance)->isClientConnected;
 350 }
 351
 352 void tcpSetBaudRate(serialPort_t *instance, uint32_t baudRate)
 353 {
 354     UNUSED(instance);
 355     UNUSED(baudRate);
 356 }
 357
 358
 359 void tcpSetMode(serialPort_t *instance, portMode_t mode)
 360 {
 361     UNUSED(instance);
 362     UNUSED(mode);
 363 }
 364
 365 static const struct serialPortVTable tcpVTable[] = {
 366     {
 367         .serialWrite = tcpWrite,
 368         .serialTotalRxWaiting = tcpTotalRxBytesWaiting,
 369         .serialTotalTxFree = tcpTotalTxBytesFree,
 370         .serialRead = tcpRead,
 371         .serialSetBaudRate = tcpSetBaudRate,
 372         .isSerialTransmitBufferEmpty = isTcpTransmitBufferEmpty,
 373         .setMode = tcpSetMode,
 374         .isConnected = tcpIsConnected,
 375         .writeBuf = tcpWritBuf,
 376         .beginWrite = NULL,
 377         .endWrite = NULL,
 378         .isIdle = NULL,
 379     }
 380 };
 381
 382 #endif