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