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