src/main/msp/msp_serial.c

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