src/main/drivers/serial_tcp.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software 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  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be 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 /*
  22  * Authors:
  23  * Dominic Clifton - Serial port abstraction, Separation of common STM32 code for cleanflight, various cleanups.
  24  * Hamasaki/Timecop - Initial baseflight code
  25 */
  26 #include <stdbool.h>
  27 #include <stdint.h>
  28 #include <stdio.h>
  29 #include <stdlib.h>
  30 #include <errno.h>
  31
  32 #include "platform.h"
  33
  34 #include "build/build_config.h"
  35
  36 #include "common/utils.h"
  37
  38 #include "io/serial.h"
  39 #include "serial_tcp.h"
  40
  41 #define BASE_PORT 5760
  42
  43 static const struct serialPortVTable tcpVTable; // Forward
  44 static tcpPort_t tcpSerialPorts[SERIAL_PORT_COUNT];
  45 static bool tcpPortInitialized[SERIAL_PORT_COUNT];
  46 static bool tcpStart = false;
  47 bool tcpIsStart(void)
  48 {
  49     return tcpStart;
  50 }
  51 static void onData(dyad_Event *e)
  52 {
  53     tcpPort_t* s = (tcpPort_t*)(e->udata);
  54     tcpDataIn(s, (uint8_t*)e->data, e->size);
  55 }
  56 static void onClose(dyad_Event *e)
  57 {
  58     tcpPort_t* s = (tcpPort_t*)(e->udata);
  59     s->clientCount--;
  60     s->conn = NULL;
  61     fprintf(stderr, "[CLS]UART%u: %d,%d\n", s->id + 1U, s->connected, s->clientCount);
  62     if (s->clientCount == 0) {
  63         s->connected = false;
  64     }
  65 }
  66 static void onAccept(dyad_Event *e)
  67 {
  68     tcpPort_t* s = (tcpPort_t*)(e->udata);
  69     fprintf(stderr, "New connection on UART%u, %d\n", s->id + 1U, s->clientCount);
  70
  71     s->connected = true;
  72     if (s->clientCount > 0) {
  73         dyad_close(e->remote);
  74         return;
  75     }
  76     s->clientCount++;
  77     fprintf(stderr, "[NEW]UART%u: %d,%d\n", s->id + 1U, s->connected, s->clientCount);
  78     s->conn = e->remote;
  79     dyad_setNoDelay(e->remote, 1);
  80     dyad_setTimeout(e->remote, 120);
  81     dyad_addListener(e->remote, DYAD_EVENT_DATA, onData, e->udata);
  82     dyad_addListener(e->remote, DYAD_EVENT_CLOSE, onClose, e->udata);
  83 }
  84 static tcpPort_t* tcpReconfigure(tcpPort_t *s, int id)
  85 {
  86     if (tcpPortInitialized[id]) {
  87         fprintf(stderr, "port is already initialized!\n");
  88         return s;
  89     }
  90
  91     if (pthread_mutex_init(&s->txLock, NULL) != 0) {
  92         fprintf(stderr, "TX mutex init failed - %d\n", errno);
  93         // TODO: clean up & re-init
  94         return NULL;
  95     }
  96     if (pthread_mutex_init(&s->rxLock, NULL) != 0) {
  97         fprintf(stderr, "RX mutex init failed - %d\n", errno);
  98         // TODO: clean up & re-init
  99         return NULL;
 100     }
 101
 102     tcpStart = true;
 103     tcpPortInitialized[id] = true;
 104
 105     s->connected = false;
 106     s->clientCount = 0;
 107     s->id = id;
 108     s->conn = NULL;
 109     s->serv = dyad_newStream();
 110     dyad_setNoDelay(s->serv, 1);
 111     dyad_addListener(s->serv, DYAD_EVENT_ACCEPT, onAccept, s);
 112
 113     if (dyad_listenEx(s->serv, NULL, BASE_PORT + id + 1, 10) == 0) {
 114         fprintf(stderr, "bind port %u for UART%u\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
 115     } else {
 116         fprintf(stderr, "bind port %u for UART%u failed!!\n", (unsigned)BASE_PORT + id + 1, (unsigned)id + 1);
 117     }
 118     return s;
 119 }
 120
 121 serialPort_t *serTcpOpen(int id, serialReceiveCallbackPtr rxCallback, void *rxCallbackData, uint32_t baudRate, portMode_e mode, portOptions_e options)
 122 {
 123     tcpPort_t *s = NULL;
 124
 125 #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)
 126     if (id >= 0 && id < SERIAL_PORT_COUNT) {
 127     s = tcpReconfigure(&tcpSerialPorts[id], id);
 128     }
 129 #endif
 130     if (!s)
 131         return NULL;
 132
 133     s->port.vTable = &tcpVTable;
 134
 135     // common serial initialisation code should move to serialPort::init()
 136     s->port.rxBufferHead = s->port.rxBufferTail = 0;
 137     s->port.txBufferHead = s->port.txBufferTail = 0;
 138     s->port.rxBufferSize = RX_BUFFER_SIZE;
 139     s->port.txBufferSize = TX_BUFFER_SIZE;
 140     s->port.rxBuffer = s->rxBuffer;
 141     s->port.txBuffer = s->txBuffer;
 142
 143     // callback works for IRQ-based RX ONLY
 144     s->port.rxCallback = rxCallback;
 145     s->port.rxCallbackData = rxCallbackData;
 146     s->port.mode = mode;
 147     s->port.baudRate = baudRate;
 148     s->port.options = options;
 149
 150     return (serialPort_t *)s;
 151 }
 152
 153 uint32_t tcpTotalRxBytesWaiting(const serialPort_t *instance)
 154 {
 155     tcpPort_t *s = (tcpPort_t*)instance;
 156     uint32_t count;
 157     pthread_mutex_lock(&s->rxLock);
 158     if (s->port.rxBufferHead >= s->port.rxBufferTail) {
 159         count = s->port.rxBufferHead - s->port.rxBufferTail;
 160     } else {
 161         count = s->port.rxBufferSize + s->port.rxBufferHead - s->port.rxBufferTail;
 162     }
 163     pthread_mutex_unlock(&s->rxLock);
 164
 165     return count;
 166 }
 167
 168 uint32_t tcpTotalTxBytesFree(const serialPort_t *instance)
 169 {
 170     tcpPort_t *s = (tcpPort_t*)instance;
 171     uint32_t bytesUsed;
 172
 173     pthread_mutex_lock(&s->txLock);
 174     if (s->port.txBufferHead >= s->port.txBufferTail) {
 175         bytesUsed = s->port.txBufferHead - s->port.txBufferTail;
 176     } else {
 177         bytesUsed = s->port.txBufferSize + s->port.txBufferHead - s->port.txBufferTail;
 178     }
 179     uint32_t bytesFree = (s->port.txBufferSize - 1) - bytesUsed;
 180     pthread_mutex_unlock(&s->txLock);
 181
 182     return bytesFree;
 183 }
 184
 185 bool isTcpTransmitBufferEmpty(const serialPort_t *instance)
 186 {
 187     tcpPort_t *s = (tcpPort_t *)instance;
 188     pthread_mutex_lock(&s->txLock);
 189     bool isEmpty = s->port.txBufferTail == s->port.txBufferHead;
 190     pthread_mutex_unlock(&s->txLock);
 191     return isEmpty;
 192 }
 193
 194 uint8_t tcpRead(serialPort_t *instance)
 195 {
 196     uint8_t ch;
 197     tcpPort_t *s = (tcpPort_t *)instance;
 198     pthread_mutex_lock(&s->rxLock);
 199
 200     ch = s->port.rxBuffer[s->port.rxBufferTail];
 201     if (s->port.rxBufferTail + 1 >= s->port.rxBufferSize) {
 202         s->port.rxBufferTail = 0;
 203     } else {
 204         s->port.rxBufferTail++;
 205     }
 206     pthread_mutex_unlock(&s->rxLock);
 207
 208     return ch;
 209 }
 210
 211 void tcpWrite(serialPort_t *instance, uint8_t ch)
 212 {
 213     tcpPort_t *s = (tcpPort_t *)instance;
 214     pthread_mutex_lock(&s->txLock);
 215
 216     s->port.txBuffer[s->port.txBufferHead] = ch;
 217     if (s->port.txBufferHead + 1 >= s->port.txBufferSize) {
 218         s->port.txBufferHead = 0;
 219     } else {
 220         s->port.txBufferHead++;
 221     }
 222     pthread_mutex_unlock(&s->txLock);
 223
 224     tcpDataOut(s);
 225 }
 226
 227 void tcpDataOut(tcpPort_t *instance)
 228 {
 229     tcpPort_t *s = (tcpPort_t *)instance;
 230     if (s->conn == NULL) return;
 231     pthread_mutex_lock(&s->txLock);
 232
 233     if (s->port.txBufferHead < s->port.txBufferTail) {
 234         // send data till end of buffer
 235         int chunk = s->port.txBufferSize - s->port.txBufferTail;
 236         dyad_write(s->conn, (const void *)&s->port.txBuffer[s->port.txBufferTail], chunk);
 237         s->port.txBufferTail = 0;
 238     }
 239     int chunk = s->port.txBufferHead - s->port.txBufferTail;
 240     if (chunk)
 241         dyad_write(s->conn, (const void*)&s->port.txBuffer[s->port.txBufferTail], chunk);
 242     s->port.txBufferTail = s->port.txBufferHead;
 243
 244     pthread_mutex_unlock(&s->txLock);
 245 }
 246
 247 void tcpDataIn(tcpPort_t *instance, uint8_t* ch, int size)
 248 {
 249     tcpPort_t *s = (tcpPort_t *)instance;
 250     pthread_mutex_lock(&s->rxLock);
 251
 252     while (size--) {
 253 //        printf("%c", *ch);
 254         s->port.rxBuffer[s->port.rxBufferHead] = *(ch++);
 255         if (s->port.rxBufferHead + 1 >= s->port.rxBufferSize) {
 256             s->port.rxBufferHead = 0;
 257         } else {
 258             s->port.rxBufferHead++;
 259         }
 260     }
 261     pthread_mutex_unlock(&s->rxLock);
 262 //    printf("\n");
 263 }
 264
 265 static const struct serialPortVTable tcpVTable = {
 266         .serialWrite = tcpWrite,
 267         .serialTotalRxWaiting = tcpTotalRxBytesWaiting,
 268         .serialTotalTxFree = tcpTotalTxBytesFree,
 269         .serialRead = tcpRead,
 270         .serialSetBaudRate = NULL,
 271         .isSerialTransmitBufferEmpty = isTcpTransmitBufferEmpty,
 272         .setMode = NULL,
 273         .setCtrlLineStateCb = NULL,
 274         .setBaudRateCb = NULL,
 275         .writeBuf = NULL,
 276         .beginWrite = NULL,
 277         .endWrite = NULL,
 278 };