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             }
 109             else {
 110                 return false;
 111             }
 112             break;
 113
 114         case MSP_HEADER_START:  // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
 115             mspPort->offset = 0;
 116             mspPort->checksum1 = 0;
 117             mspPort->checksum2 = 0;
 118             switch (c) {
 119                 case 'M':
 120                     mspPort->c_state = MSP_HEADER_M;
 121                     mspPort->mspVersion = MSP_V1;
 122                     break;
 123                 case 'X':
 124                     mspPort->c_state = MSP_HEADER_X;
 125                     mspPort->mspVersion = MSP_V2_NATIVE;
 126                     break;
 127                 default:
 128                     mspPort->c_state = MSP_IDLE;
 129                     break;
 130             }
 131             break;
 132
 133         case MSP_HEADER_M:      // Waiting for '<' / '>'
 134             mspPort->c_state = MSP_HEADER_V1;
 135             switch (c) {
 136                 case '<':
 137                     mspPort->packetType = MSP_PACKET_COMMAND;
 138                     break;
 139                 case '>':
 140                     mspPort->packetType = MSP_PACKET_REPLY;
 141                     break;
 142                 default:
 143                     mspPort->c_state = MSP_IDLE;
 144                     break;
 145             }
 146             break;
 147
 148         case MSP_HEADER_X:
 149             mspPort->c_state = MSP_HEADER_V2_NATIVE;
 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->c_state = MSP_IDLE;
 159                     break;
 160             }
 161             break;
 162
 163         case MSP_HEADER_V1:     // Now receive v1 header (size/cmd), this is already checksummable
 164             mspPort->inBuf[mspPort->offset++] = c;
 165             mspPort->checksum1 ^= c;
 166             if (mspPort->offset == sizeof(mspHeaderV1_t)) {
 167                 mspHeaderV1_t * hdr = (mspHeaderV1_t *)&mspPort->inBuf[0];
 168                 // Check incoming buffer size limit
 169                 if (hdr->size > MSP_PORT_INBUF_SIZE) {
 170                     mspPort->c_state = MSP_IDLE;
 171                 }
 172                 else if (hdr->cmd == MSP_V2_FRAME_ID) {
 173                     // MSPv1 payload must be big enough to hold V2 header + extra checksum
 174                     if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
 175                         mspPort->mspVersion = MSP_V2_OVER_V1;
 176                         mspPort->c_state = MSP_HEADER_V2_OVER_V1;
 177                     }
 178                     else {
 179                         mspPort->c_state = MSP_IDLE;
 180                     }
 181                 }
 182                 else {
 183                     mspPort->dataSize = hdr->size;
 184                     mspPort->cmdMSP = hdr->cmd;
 185                     mspPort->cmdFlags = 0;
 186                     mspPort->offset = 0;                // re-use buffer
 187                     mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1;    // If no payload - jump to checksum byte
 188                 }
 189             }
 190             break;
 191
 192         case MSP_PAYLOAD_V1:
 193             mspPort->inBuf[mspPort->offset++] = c;
 194             mspPort->checksum1 ^= c;
 195             if (mspPort->offset == mspPort->dataSize) {
 196                 mspPort->c_state = MSP_CHECKSUM_V1;
 197             }
 198             break;
 199
 200         case MSP_CHECKSUM_V1:
 201             if (mspPort->checksum1 == c) {
 202                 mspPort->c_state = MSP_COMMAND_RECEIVED;
 203             } else {
 204                 mspPort->c_state = MSP_IDLE;
 205             }
 206             break;
 207
 208         case MSP_HEADER_V2_OVER_V1:     // V2 header is part of V1 payload - we need to calculate both checksums now
 209             mspPort->inBuf[mspPort->offset++] = c;
 210             mspPort->checksum1 ^= c;
 211             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 212             if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
 213                 mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
 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             break;
 221
 222         case MSP_PAYLOAD_V2_OVER_V1:
 223             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 224             mspPort->checksum1 ^= c;
 225             mspPort->inBuf[mspPort->offset++] = c;
 226
 227             if (mspPort->offset == mspPort->dataSize) {
 228                 mspPort->c_state = MSP_CHECKSUM_V2_OVER_V1;
 229             }
 230             break;
 231
 232         case MSP_CHECKSUM_V2_OVER_V1:
 233             mspPort->checksum1 ^= c;
 234             if (mspPort->checksum2 == c) {
 235                 mspPort->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
 236             } else {
 237                 mspPort->c_state = MSP_IDLE;
 238             }
 239             break;
 240
 241         case MSP_HEADER_V2_NATIVE:
 242             mspPort->inBuf[mspPort->offset++] = c;
 243             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 244             if (mspPort->offset == sizeof(mspHeaderV2_t)) {
 245                 mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[0];
 246                 mspPort->dataSize = hdrv2->size;
 247                 mspPort->cmdMSP = hdrv2->cmd;
 248                 mspPort->cmdFlags = hdrv2->flags;
 249                 mspPort->offset = 0;                // re-use buffer
 250                 mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
 251             }
 252             break;
 253
 254         case MSP_PAYLOAD_V2_NATIVE:
 255             mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
 256             mspPort->inBuf[mspPort->offset++] = c;
 257
 258             if (mspPort->offset == mspPort->dataSize) {
 259                 mspPort->c_state = MSP_CHECKSUM_V2_NATIVE;
 260             }
 261             break;
 262
 263         case MSP_CHECKSUM_V2_NATIVE:
 264             if (mspPort->checksum2 == c) {
 265                 mspPort->c_state = MSP_COMMAND_RECEIVED;
 266             } else {
 267                 mspPort->c_state = MSP_IDLE;
 268             }
 269             break;
 270     }
 271
 272     return true;
 273 }
 274
 275 static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len)
 276 {
 277     while (len-- > 0) {
 278         checksum ^= *data++;
 279     }
 280     return checksum;
 281 }
 282
 283 #define JUMBO_FRAME_SIZE_LIMIT 255
 284 static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen)
 285 {
 286     // We are allowed to send out the response if
 287     //  a) TX buffer is completely empty (we are talking to well-behaving party that follows request-response scheduling;
 288     //     this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
 289     //  b) Response fits into TX buffer
 290     const int totalFrameLength = hdrLen + dataLen + crcLen;
 291     if (!isSerialTransmitBufferEmpty(msp->port) && ((int)serialTxBytesFree(msp->port) < totalFrameLength))
 292         return 0;
 293
 294     // Transmit frame
 295     serialBeginWrite(msp->port);
 296     serialWriteBuf(msp->port, hdr, hdrLen);
 297     serialWriteBuf(msp->port, data, dataLen);
 298     serialWriteBuf(msp->port, crc, crcLen);
 299     serialEndWrite(msp->port);
 300
 301     return totalFrameLength;
 302 }
 303
 304 static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e mspVersion)
 305 {
 306     static const uint8_t mspMagic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
 307     const int dataLen = sbufBytesRemaining(&packet->buf);
 308     uint8_t hdrBuf[16] = { '$', mspMagic[mspVersion], packet->result == MSP_RESULT_ERROR ? '!' : '>'};
 309     uint8_t crcBuf[2];
 310     uint8_t checksum;
 311     int hdrLen = 3;
 312     int crcLen = 0;
 313
 314     #define V1_CHECKSUM_STARTPOS 3
 315     if (mspVersion == MSP_V1) {
 316         mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
 317         hdrLen += sizeof(mspHeaderV1_t);
 318         hdrV1->cmd = packet->cmd;
 319
 320         // Add JUMBO-frame header if necessary
 321         if (dataLen >= JUMBO_FRAME_SIZE_LIMIT) {
 322             mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen];
 323             hdrLen += sizeof(mspHeaderJUMBO_t);
 324
 325             hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
 326             hdrJUMBO->size = dataLen;
 327         }
 328         else {
 329             hdrV1->size = dataLen;
 330         }
 331
 332         // Pre-calculate CRC
 333         checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
 334         checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
 335         crcBuf[crcLen++] = checksum;
 336     }
 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         }
 356         else {
 357             hdrV1->size = v1PayloadSize;
 358         }
 359
 360         // Fill V2 header
 361         hdrV2->flags = packet->flags;
 362         hdrV2->cmd = packet->cmd;
 363         hdrV2->size = dataLen;
 364
 365         // V2 CRC: only V2 header + data payload
 366         checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
 367         checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
 368         crcBuf[crcLen++] = checksum;
 369
 370         // V1 CRC: All headers + data payload + V2 CRC byte
 371         checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
 372         checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
 373         checksum = mspSerialChecksumBuf(checksum, crcBuf, crcLen);
 374         crcBuf[crcLen++] = checksum;
 375     }
 376     else if (mspVersion == MSP_V2_NATIVE) {
 377         mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
 378         hdrLen += sizeof(mspHeaderV2_t);
 379
 380         hdrV2->flags = packet->flags;
 381         hdrV2->cmd = packet->cmd;
 382         hdrV2->size = dataLen;
 383
 384         checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
 385         checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
 386         crcBuf[crcLen++] = checksum;
 387     }
 388     else {
 389         // Shouldn't get here
 390         return 0;
 391     }
 392
 393     // Send the frame
 394     return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen);
 395 }
 396
 397 static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
 398 {
 399     static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];
 400
 401     mspPacket_t reply = {
 402         .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
 403         .cmd = -1,
 404         .flags = 0,
 405         .result = 0,
 406         .direction = MSP_DIRECTION_REPLY,
 407     };
 408     uint8_t *outBufHead = reply.buf.ptr;
 409
 410     mspPacket_t command = {
 411         .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
 412         .cmd = msp->cmdMSP,
 413         .flags = msp->cmdFlags,
 414         .result = 0,
 415         .direction = MSP_DIRECTION_REQUEST,
 416     };
 417
 418     mspPostProcessFnPtr mspPostProcessFn = NULL;
 419     const mspResult_e status = mspProcessCommandFn(msp->descriptor, &command, &reply, &mspPostProcessFn);
 420
 421     if (status != MSP_RESULT_NO_REPLY) {
 422         sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
 423         mspSerialEncode(msp, &reply, msp->mspVersion);
 424     }
 425
 426     return mspPostProcessFn;
 427 }
 428
 429 static void mspEvaluateNonMspData(mspPort_t * mspPort, uint8_t receivedChar)
 430 {
 431    if (receivedChar == serialConfig()->reboot_character) {
 432         mspPort->pendingRequest = MSP_PENDING_BOOTLOADER_ROM;
 433 #ifdef USE_CLI
 434    } else if (receivedChar == '#') {
 435         mspPort->pendingRequest = MSP_PENDING_CLI;
 436 #endif
 437 #if defined(USE_FLASH_BOOT_LOADER)
 438    } else if (receivedChar == 'F') {
 439         mspPort->pendingRequest = MSP_PENDING_BOOTLOADER_FLASH;
 440 #endif
 441     }
 442 }
 443
 444 static void mspProcessPendingRequest(mspPort_t * mspPort)
 445 {
 446     // If no request is pending or 100ms guard time has not elapsed - do nothing
 447     if ((mspPort->pendingRequest == MSP_PENDING_NONE) || (millis() - mspPort->lastActivityMs < 100)) {
 448         return;
 449     }
 450
 451     switch(mspPort->pendingRequest) {
 452     case MSP_PENDING_BOOTLOADER_ROM:
 453         systemResetToBootloader(BOOTLOADER_REQUEST_ROM);
 454
 455         break;
 456 #if defined(USE_FLASH_BOOT_LOADER)
 457     case MSP_PENDING_BOOTLOADER_FLASH:
 458         systemResetToBootloader(BOOTLOADER_REQUEST_FLASH);
 459
 460         break;
 461 #endif
 462 #ifdef USE_CLI
 463     case MSP_PENDING_CLI:
 464         cliEnter(mspPort->port);
 465         break;
 466 #endif
 467
 468     default:
 469         break;
 470     }
 471 }
 472
 473 static void mspSerialProcessReceivedReply(mspPort_t *msp, mspProcessReplyFnPtr mspProcessReplyFn)
 474 {
 475     mspPacket_t reply = {
 476         .buf = {
 477             .ptr = msp->inBuf,
 478             .end = msp->inBuf + msp->dataSize,
 479         },
 480         .cmd = msp->cmdMSP,
 481         .result = 0,
 482     };
 483
 484     mspProcessReplyFn(&reply);
 485
 486     msp->c_state = MSP_IDLE;
 487 }
 488
 489 /*
 490  * Process MSP commands from serial ports configured as MSP ports.
 491  *
 492  * Called periodically by the scheduler.
 493  */
 494 void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn)
 495 {
 496     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
 497         mspPort_t * const mspPort = &mspPorts[portIndex];
 498         if (!mspPort->port) {
 499             continue;
 500         }
 501
 502         mspPostProcessFnPtr mspPostProcessFn = NULL;
 503
 504         if (serialRxBytesWaiting(mspPort->port)) {
 505             // There are bytes incoming - abort pending request
 506             mspPort->lastActivityMs = millis();
 507             mspPort->pendingRequest = MSP_PENDING_NONE;
 508
 509             while (serialRxBytesWaiting(mspPort->port)) {
 510                 const uint8_t c = serialRead(mspPort->port);
 511                 const bool consumed = mspSerialProcessReceivedData(mspPort, c);
 512
 513                 if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
 514                     mspEvaluateNonMspData(mspPort, c);
 515                 }
 516
 517                 if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
 518                     if (mspPort->packetType == MSP_PACKET_COMMAND) {
 519                         mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
 520                     } else if (mspPort->packetType == MSP_PACKET_REPLY) {
 521                         mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
 522                     }
 523
 524                     mspPort->c_state = MSP_IDLE;
 525                     break; // process one command at a time so as not to block.
 526                 }
 527             }
 528
 529             if (mspPostProcessFn) {
 530                 waitForSerialPortToFinishTransmitting(mspPort->port);
 531                 mspPostProcessFn(mspPort->port);
 532             }
 533         }
 534         else {
 535             mspProcessPendingRequest(mspPort);
 536         }
 537     }
 538 }
 539
 540 bool mspSerialWaiting(void)
 541 {
 542     for (uint8_t portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
 543         mspPort_t * const mspPort = &mspPorts[portIndex];
 544         if (!mspPort->port) {
 545             continue;
 546         }
 547
 548         if (serialRxBytesWaiting(mspPort->port)) {
 549             return true;
 550         }
 551     }
 552     return false;
 553 }
 554
 555 void mspSerialInit(void)
 556 {
 557     memset(mspPorts, 0, sizeof(mspPorts));
 558     mspSerialAllocatePorts();
 559 }
 560
 561 int mspSerialPush(serialPortIdentifier_e port, uint8_t cmd, uint8_t *data, int datalen, mspDirection_e direction)
 562 {
 563     int ret = 0;
 564
 565     for (int portIndex = 0; portIndex < MAX_MSP_PORT_COUNT; portIndex++) {
 566         mspPort_t * const mspPort = &mspPorts[portIndex];
 567
 568         // XXX Kludge!!! Avoid zombie VCP port (avoid VCP entirely for now)
 569         if (!mspPort->port || mspPort->port->identifier == SERIAL_PORT_USB_VCP || (port != SERIAL_PORT_NONE && 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 }