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