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