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