src/main/msp/msp_serial.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdio.h>
  21 #include <ctype.h>
  22 #include <string.h>
  23
  24 #include "platform.h"
  25
  26 #include "build/debug.h"
  27
  28 #include "common/streambuf.h"
  29 #include "common/utils.h"
  30 #include "common/maths.h"
  31 #include "common/crc.h"
  32
  33 #include "drivers/system.h"
  34 #include "drivers/serial.h"
  35
  36 #include "io/serial.h"
  37 #include "fc/cli.h"
  38
  39 #include "msp/msp.h"
  40 #include "msp/msp_serial.h"
  41
  42 static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
  43
  44
  45 void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
  46 {
  47     memset(mspPortToReset, 0, sizeof(mspPort_t));
  48
  49     mspPortToReset->port = serialPort;
  50 }
  51
  52 void mspSerialAllocatePorts(void)
  53 {
  54     uint8_t portIndex = 0;
  55     serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_MSP);
  56     while (portConfig && portIndex < MAX_MSP_PORT_COUNT) {
  57         mspPort_t *mspPort = &mspPorts[portIndex];
  58         if (mspPort->port) {
  59             portIndex++;
  60             continue;
  61         }
  62
  63         serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, NULL, baudRates[portConfig->msp_baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
  64         if (serialPort) {
  65             resetMspPort(mspPort, serialPort);
  66             portIndex++;
  67         }
  68
  69         portConfig = findNextSerialPortConfig(FUNCTION_MSP);
  70     }
  71 }
  72
  73 void mspSerialReleasePortIfAllocated(serialPort_t *serialPort)
  74 {
  75     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
  76         mspPort_t *candidateMspPort = &mspPorts[portIndex];
  77         if (candidateMspPort->port == serialPort) {
  78             closeSerialPort(serialPort);
  79             memset(candidateMspPort, 0, sizeof(mspPort_t));
  80         }
  81     }
  82 }
  83
  84 static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
  85 {
  86     switch (mspPort->c_state) {
  87         default:
  88         case MSP_IDLE:      // Waiting for '$' character
  89             if (c == '$') {
  90                 mspPort->mspVersion = MSP_V1;
  91                 mspPort->c_state = MSP_HEADER_START;
  92             }
  93             else {
  94                 return false;
  95             }
  96             break;
  97
  98         case MSP_HEADER_START:  // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
  99             switch (c) {
 100                 case 'M':
 101                     mspPort->c_state = MSP_HEADER_M;
 102                     break;
 103                 case 'X':
 104                     mspPort->c_state = MSP_HEADER_X;
 105                     break;
 106                 default:
 107                     mspPort->c_state = MSP_IDLE;
 108                     break;
 109             }
 110             break;
 111
 112         case MSP_HEADER_M:      // Waiting for '<'
 113             if (c == '<') {
 114                 mspPort->offset = 0;
 115                 mspPort->checksum1 = 0;
 116                 mspPort->checksum2 = 0;
 117                 mspPort->c_state = MSP_HEADER_V1;
 118             }
 119             else {
 120                 mspPort->c_state = MSP_IDLE;
 121             }
 122             break;
 123
 124         case MSP_HEADER_X:
 125             if (c == '<') {
 126                 mspPort->offset = 0;
 127                 mspPort->checksum2 = 0;
 128                 mspPort->mspVersion = MSP_V2_NATIVE;
 129                 mspPort->c_state = MSP_HEADER_V2_NATIVE;
 130             }
 131             else {
 132                 mspPort->c_state = MSP_IDLE;
 133             }
 134             break;
 135
 136         case MSP_HEADER_V1:     // Now receive v1 header (size/cmd), this is already checksummable
 137             mspPort->inBuf[mspPort->offset++] = c;
 138             mspPort->checksum1 ^= c;
 139             if (mspPort->offset == sizeof(mspHeaderV1_t)) {
 140                 mspHeaderV1_t * hdr = (mspHeaderV1_t *)&mspPort->inBuf[0];
 141                 // Check incoming buffer size limit
 142                 if (hdr->size > MSP_PORT_INBUF_SIZE) {
 143                     mspPort->c_state = MSP_IDLE;
 144                 }
 145                 else if (hdr->cmd == MSP_V2_FRAME_ID) {
 146                     // MSPv1 payload must be big enough to hold V2 header + extra checksum
 147                     if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
 148                         mspPort->mspVersion = MSP_V2_OVER_V1;
 149                         mspPort->c_state = MSP_HEADER_V2_OVER_V1;
 150                     }
 151                     else {
 152                         mspPort->c_state = MSP_IDLE;
 153                     }
 154                 }
 155                 else {
 156                     mspPort->dataSize = hdr->size;
 157                     mspPort->cmdMSP = hdr->cmd;
 158                     mspPort->cmdFlags = 0;
 159                     mspPort->offset = 0;                // re-use buffer
 160                     mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1;    // If no payload - jump to checksum byte
 161                 }
 162             }
 163             break;
 164
 165         case MSP_PAYLOAD_V1:
 166             mspPort->inBuf[mspPort->offset++] = c;
 167             mspPort->checksum1 ^= c;
 168             if (mspPort->offset == mspPort->dataSize) {
 169                 mspPort->c_state = MSP_CHECKSUM_V1;
 170             }
 171             break;
 172
 173         case MSP_CHECKSUM_V1:
 174             if (mspPort->checksum1 == c) {
 175                 mspPort->c_state = MSP_COMMAND_RECEIVED;
 176                 SD(fprintf(stderr, "[MSPV1] Command received\n"));
 177             } else {
 178                 mspPort->c_state = MSP_IDLE;
 179                 SD(fprintf(stderr, "[MSPV1] Checksum error!\n"));
 180             }
 181             break;
 182
 183         case MSP_HEADER_V2_OVER_V1:     // V2 header is part of V1 payload - we need to calculate both checksums now
 184             mspPort->inBuf[mspPort->offset++] = c;
 185             mspPort->checksum1 ^= c;
 186             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 187             if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
 188                 mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
 189                 mspPort->dataSize = hdrv2->size;
 190
 191                 // Check for potential buffer overflow
 192                 if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
 193                     mspPort->c_state = MSP_IDLE;
 194                 }
 195                 else {
 196                     mspPort->cmdMSP = hdrv2->cmd;
 197                     mspPort->cmdFlags = hdrv2->flags;
 198                     mspPort->offset = 0;                // re-use buffer
 199                     mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
 200                 }
 201             }
 202             break;
 203
 204         case MSP_PAYLOAD_V2_OVER_V1:
 205             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 206             mspPort->checksum1 ^= c;
 207             mspPort->inBuf[mspPort->offset++] = c;
 208
 209             if (mspPort->offset == mspPort->dataSize) {
 210                 mspPort->c_state = MSP_CHECKSUM_V2_OVER_V1;
 211             }
 212             break;
 213
 214         case MSP_CHECKSUM_V2_OVER_V1:
 215             mspPort->checksum1 ^= c;
 216             if (mspPort->checksum2 == c) {
 217                 mspPort->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
 218             } else {
 219                 mspPort->c_state = MSP_IDLE;
 220             }
 221             break;
 222
 223         case MSP_HEADER_V2_NATIVE:
 224             mspPort->inBuf[mspPort->offset++] = c;
 225             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 226             if (mspPort->offset == sizeof(mspHeaderV2_t)) {
 227                 mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[0];
 228
 229                 // Check for potential buffer overflow
 230                 if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
 231                     mspPort->c_state = MSP_IDLE;
 232                     SD(fprintf(stderr, "[MSPV2] Potential buffer overflow!\n"));
 233                 }
 234                 else {
 235                     mspPort->dataSize = hdrv2->size;
 236                     mspPort->cmdMSP = hdrv2->cmd;
 237                     mspPort->cmdFlags = hdrv2->flags;
 238                     mspPort->offset = 0;                // re-use buffer
 239                     mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
 240                 }
 241             }
 242             break;
 243
 244         case MSP_PAYLOAD_V2_NATIVE:
 245             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 246             mspPort->inBuf[mspPort->offset++] = c;
 247
 248             if (mspPort->offset == mspPort->dataSize) {
 249                 mspPort->c_state = MSP_CHECKSUM_V2_NATIVE;
 250             }
 251             break;
 252
 253         case MSP_CHECKSUM_V2_NATIVE:
 254             if (mspPort->checksum2 == c) {
 255                 mspPort->c_state = MSP_COMMAND_RECEIVED;
 256                 SD(fprintf(stderr, "[MSPV2] command received!\n"));
 257             } else {
 258                 SD(fprintf(stderr, "[MSPV2] Checksum error!\n"));
 259                 mspPort->c_state = MSP_IDLE;
 260             }
 261             break;
 262     }
 263
 264     return true;
 265 }
 266
 267 static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len)
 268 {
 269     while (len-- > 0) {
 270         checksum ^= *data++;
 271     }
 272     return checksum;
 273 }
 274
 275 #define JUMBO_FRAME_SIZE_LIMIT 255
 276 static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen)
 277 {
 278     // MSP port might be turned into a CLI port, which will make
 279     // msp->port become NULL.
 280     serialPort_t *port = msp->port;
 281     if (!port) {
 282         return 0;
 283     }
 284     // VSP MSP port might be unconnected. To prevent blocking - check if it's connected first
 285     if (!serialIsConnected(port)) {
 286         return 0;
 287     }
 288
 289     // We are allowed to send out the response if
 290     //  a) TX buffer is completely empty (we are talking to well-behaving party that follows request-response scheduling;
 291     //     this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
 292     //  b) Response fits into TX buffer
 293     const int totalFrameLength = hdrLen + dataLen + crcLen;
 294     if (!isSerialTransmitBufferEmpty(port) && ((int)serialTxBytesFree(port) < totalFrameLength))
 295         return 0;
 296
 297     // Transmit frame
 298     serialBeginWrite(port);
 299     serialWriteBuf(port, hdr, hdrLen);
 300     serialWriteBuf(port, data, dataLen);
 301     serialWriteBuf(port, crc, crcLen);
 302     serialEndWrite(port);
 303
 304     return totalFrameLength;
 305 }
 306
 307 static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e mspVersion)
 308 {
 309     static const uint8_t mspMagic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
 310     const int dataLen = sbufBytesRemaining(&packet->buf);
 311     uint8_t hdrBuf[16] = { '$', mspMagic[mspVersion], packet->result == MSP_RESULT_ERROR ? '!' : '>'};
 312     uint8_t crcBuf[2];
 313     int hdrLen = 3;
 314     int crcLen = 0;
 315
 316     #define V1_CHECKSUM_STARTPOS 3
 317     if (mspVersion == MSP_V1) {
 318         mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
 319         hdrLen += sizeof(mspHeaderV1_t);
 320         hdrV1->cmd = packet->cmd;
 321
 322         // Add JUMBO-frame header if necessary
 323         if (dataLen >= JUMBO_FRAME_SIZE_LIMIT) {
 324             mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen];
 325             hdrLen += sizeof(mspHeaderJUMBO_t);
 326
 327             hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
 328             hdrJUMBO->size = dataLen;
 329         }
 330         else {
 331             hdrV1->size = dataLen;
 332         }
 333
 334         // Pre-calculate CRC
 335         crcBuf[crcLen] = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
 336         crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen);
 337         crcLen++;
 338     }
 339     else if (mspVersion == MSP_V2_OVER_V1) {
 340         mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
 341
 342         hdrLen += sizeof(mspHeaderV1_t);
 343
 344         mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
 345         hdrLen += sizeof(mspHeaderV2_t);
 346
 347         const int v1PayloadSize = sizeof(mspHeaderV2_t) + dataLen + 1;  // MSPv2 header + data payload + MSPv2 checksum
 348         hdrV1->cmd = MSP_V2_FRAME_ID;
 349
 350         // Add JUMBO-frame header if necessary
 351         if (v1PayloadSize >= JUMBO_FRAME_SIZE_LIMIT) {
 352             mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen];
 353             hdrLen += sizeof(mspHeaderJUMBO_t);
 354
 355             hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
 356             hdrJUMBO->size = v1PayloadSize;
 357         }
 358         else {
 359             hdrV1->size = v1PayloadSize;
 360         }
 361
 362         // Fill V2 header
 363         hdrV2->flags = packet->flags;
 364         hdrV2->cmd = packet->cmd;
 365         hdrV2->size = dataLen;
 366
 367         // V2 CRC: only V2 header + data payload
 368         crcBuf[crcLen] = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
 369         crcBuf[crcLen] = crc8_dvb_s2_update(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen);
 370         crcLen++;
 371
 372         // V1 CRC: All headers + data payload + V2 CRC byte
 373         crcBuf[crcLen] = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
 374         crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen);
 375         crcBuf[crcLen] = mspSerialChecksumBuf(crcBuf[crcLen], crcBuf, crcLen);
 376         crcLen++;
 377     }
 378     else if (mspVersion == MSP_V2_NATIVE) {
 379         mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
 380         hdrLen += sizeof(mspHeaderV2_t);
 381
 382         hdrV2->flags = packet->flags;
 383         hdrV2->cmd = packet->cmd;
 384         hdrV2->size = dataLen;
 385
 386         crcBuf[crcLen] = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
 387         crcBuf[crcLen] = crc8_dvb_s2_update(crcBuf[crcLen], sbufPtr(&packet->buf), dataLen);
 388         crcLen++;
 389     }
 390     else {
 391         // Shouldn't get here
 392         return 0;
 393     }
 394
 395     // Send the frame
 396     return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen);
 397 }
 398
 399 static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
 400 {
 401     uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
 402
 403     mspPacket_t reply = {
 404         .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
 405         .cmd = -1,
 406         .flags = 0,
 407         .result = 0,
 408     };
 409     uint8_t *outBufHead = reply.buf.ptr;
 410
 411     mspPacket_t command = {
 412         .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
 413         .cmd = msp->cmdMSP,
 414         .flags = msp->cmdFlags,
 415         .result = 0,
 416     };
 417
 418     mspPostProcessFnPtr mspPostProcessFn = NULL;
 419     const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);
 420
 421     if (status != MSP_RESULT_NO_REPLY) {
 422         sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
 423         mspSerialEncode(msp, &reply, msp->mspVersion);
 424     }
 425
 426     msp->c_state = MSP_IDLE;
 427     return mspPostProcessFn;
 428 }
 429
 430 static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar)
 431 {
 432     if (receivedChar == '#') {
 433         mspPort->pendingRequest = MSP_PENDING_CLI;
 434         return;
 435     }
 436
 437     if (receivedChar == serialConfig()->reboot_character) {
 438         mspPort->pendingRequest = MSP_PENDING_BOOTLOADER;
 439         return;
 440     }
 441 }
 442
 443 static void mspProcessPendingRequest(mspPort_t * mspPort)
 444 {
 445     // If no request is pending or 100ms guard time has not elapsed - do nothing
 446     if ((mspPort->pendingRequest == MSP_PENDING_NONE) || (millis() - mspPort->lastActivityMs < 100)) {
 447         return;
 448     }
 449
 450     switch(mspPort->pendingRequest) {
 451         case MSP_PENDING_BOOTLOADER:
 452             systemResetToBootloader();
 453             break;
 454
 455         case MSP_PENDING_CLI:
 456             if (!cliMode) {
 457                 // When we enter CLI mode - disable this MSP port. Don't care about preserving the port since CLI can only be exited via reboot
 458                 cliEnter(mspPort->port);
 459                 mspPort->port = NULL;
 460             }
 461             break;
 462
 463         default:
 464             break;
 465     }
 466 }
 467
 468 void mspSerialProcessOnePort(mspPort_t * const mspPort, mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
 469 {
 470     mspPostProcessFnPtr mspPostProcessFn = NULL;
 471
 472     if (serialRxBytesWaiting(mspPort->port)) {
 473         // There are bytes incoming - abort pending request
 474         mspPort->lastActivityMs = millis();
 475         mspPort->pendingRequest = MSP_PENDING_NONE;
 476
 477         // Process incoming bytes
 478         while (serialRxBytesWaiting(mspPort->port)) {
 479             const uint8_t c = serialRead(mspPort->port);
 480             const bool consumed = mspSerialProcessReceivedData(mspPort, c);
 481
 482             //SD(fprintf(stderr, "[MSP]: received char: %02x (%c) state: %i\n", c, isprint(c) ? c : '.', mspPort->c_state));
 483             if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
 484                 mspEvaluateNonMspData(mspPort, c);
 485             }
 486
 487             if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
 488                 mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
 489                 break; // process one command at a time so as not to block.
 490             }
 491         }
 492
 493         if (mspPostProcessFn) {
 494             waitForSerialPortToFinishTransmitting(mspPort->port);
 495             mspPostProcessFn(mspPort->port);
 496         }
 497     }
 498     else {
 499         mspProcessPendingRequest(mspPort);
 500     }
 501 }
 502
 503 /*
 504  * Process MSP commands from serial ports configured as MSP ports.
 505  *
 506  * Called periodically by the scheduler.
 507  */
 508 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn)
 509 {
 510     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
 511         mspPort_t * const mspPort = &mspPorts[portIndex];
 512         if (mspPort->port) {
 513             mspSerialProcessOnePort(mspPort, evaluateNonMspData, mspProcessCommandFn);
 514         }
 515     }
 516 }
 517
 518 void mspSerialInit(void)
 519 {
 520     memset(mspPorts, 0, sizeof(mspPorts));
 521     mspSerialAllocatePorts();
 522 }
 523
 524 int mspSerialPushPort(uint16_t cmd, const uint8_t *data, int datalen, mspPort_t *mspPort, mspVersion_e version)
 525 {
 526     uint8_t pushBuf[MSP_PORT_OUTBUF_SIZE];
 527
 528     mspPacket_t push = {
 529         .buf = { .ptr = pushBuf, .end = ARRAYEND(pushBuf), },
 530         .cmd = cmd,
 531         .result = 0,
 532     };
 533
 534     sbufWriteData(&push.buf, data, datalen);
 535
 536     sbufSwitchToReader(&push.buf, pushBuf);
 537
 538     return mspSerialEncode(mspPort, &push, version);
 539 }
 540
 541 int mspSerialPushVersion(uint8_t cmd, const uint8_t *data, int datalen, mspVersion_e version)
 542 {
 543     int ret = 0;
 544
 545     for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
 546         mspPort_t * const mspPort = &mspPorts[portIndex];
 547         if (!mspPort->port) {
 548             continue;
 549         }
 550
 551         // Avoid unconnected ports (only VCP for now)
 552         if (!serialIsConnected(mspPort->port)) {
 553             continue;
 554         }
 555
 556         ret = mspSerialPushPort(cmd, data, datalen, mspPort, version);
 557     }
 558     return ret; // return the number of bytes written
 559 }
 560
 561 int mspSerialPush(uint8_t cmd, const uint8_t *data, int datalen)
 562 {
 563     return mspSerialPushVersion(cmd, data, datalen, MSP_V1);
 564 }
 565
 566 uint32_t mspSerialTxBytesFree(serialPort_t *port)
 567 {
 568    return serialTxBytesFree(port);
 569 }
 570
 571 mspPort_t * mspSerialPortFind(const serialPort_t *serialPort)
 572 {
 573     for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
 574         mspPort_t * mspPort = &mspPorts[portIndex];
 575         if (mspPort->port == serialPort) {
 576             return mspPort;
 577         }
 578     }
 579     return NULL;
 580 }