src/main/io/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 <stdlib.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #include "build_config.h"
  26
  27 #include "drivers/system.h"
  28 #include "drivers/gpio.h"
  29 #include "drivers/timer.h"
  30 #include "drivers/serial.h"
  31 #include "drivers/serial_softserial.h"
  32 #include "drivers/serial_uart.h"
  33 #include "drivers/serial_usb_vcp.h"
  34
  35 #include "io/serial.h"
  36 #include "serial_cli.h"
  37 #include "serial_msp.h"
  38
  39 #include "config/config.h"
  40
  41 #ifdef TELEMETRY
  42 #include "telemetry/telemetry.h"
  43 #endif
  44
  45 void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate);
  46
  47 void mspInit(serialConfig_t *serialConfig);
  48 void cliInit(serialConfig_t *serialConfig);
  49
  50 // this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask.
  51 const serialPortFunctionScenario_e serialPortScenarios[SERIAL_PORT_SCENARIO_COUNT] = {
  52     SCENARIO_UNUSED,
  53
  54     // common scenarios in order of importance
  55     SCENARIO_MSP_CLI_TELEMETRY_GPS_PASTHROUGH,
  56     SCENARIO_GPS_ONLY,
  57     SCENARIO_SERIAL_RX_ONLY,
  58     SCENARIO_TELEMETRY_ONLY,
  59
  60     // other scenarios
  61     SCENARIO_MSP_CLI_GPS_PASTHROUGH,
  62     SCENARIO_CLI_ONLY,
  63     SCENARIO_GPS_PASSTHROUGH_ONLY,
  64     SCENARIO_MSP_ONLY,
  65     SCENARIO_SMARTPORT_TELEMETRY_ONLY,
  66
  67     SCENARIO_BLACKBOX_ONLY,
  68     SCENARIO_MSP_CLI_BLACKBOX_GPS_PASTHROUGH
  69 };
  70
  71 static serialConfig_t *serialConfig;
  72 static serialPort_t *serialPorts[SERIAL_PORT_COUNT];
  73
  74 #ifdef STM32F303xC
  75 static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
  76 #ifdef USE_VCP
  77     {SERIAL_PORT_USB_VCP,     NULL, SCENARIO_UNUSED, FUNCTION_NONE},
  78 #endif
  79 #ifdef USE_USART1
  80     {SERIAL_PORT_USART1,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
  81 #endif
  82 #ifdef USE_USART2
  83     {SERIAL_PORT_USART2,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
  84 #endif
  85 #ifdef USE_USART3
  86     {SERIAL_PORT_USART3,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
  87 #endif
  88 };
  89
  90 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
  91 #ifdef USE_VCP
  92     {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
  93 #endif
  94 #ifdef USE_USART1
  95     {SERIAL_PORT_USART1,        9600,   115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
  96 #endif
  97 #ifdef USE_USART2
  98     {SERIAL_PORT_USART2,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
  99 #endif
 100 #ifdef USE_USART3
 101     {SERIAL_PORT_USART3,        9600,   115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 102 #endif
 103 };
 104
 105 #else
 106
 107 #ifdef CC3D
 108 static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
 109 #ifdef USE_VCP
 110     {SERIAL_PORT_USB_VCP,     NULL, SCENARIO_UNUSED, FUNCTION_NONE},
 111 #endif
 112     {SERIAL_PORT_USART1,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
 113     {SERIAL_PORT_USART3,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
 114     {SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE}
 115 };
 116
 117 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
 118 #ifdef USE_VCP
 119     {SERIAL_PORT_USB_VCP,       9600,   115200,   SPF_NONE },
 120 #endif
 121     {SERIAL_PORT_USART1,        9600, 115200,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 122     {SERIAL_PORT_USART3,        9600, 115200,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 123     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
 124 };
 125 #else
 126
 127 static serialPortFunction_t serialPortFunctions[SERIAL_PORT_COUNT] = {
 128     {SERIAL_PORT_USART1,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
 129     {SERIAL_PORT_USART2,      NULL, SCENARIO_UNUSED, FUNCTION_NONE},
 130 #if (SERIAL_PORT_COUNT > 2)
 131     {SERIAL_PORT_SOFTSERIAL1, NULL, SCENARIO_UNUSED, FUNCTION_NONE},
 132     {SERIAL_PORT_SOFTSERIAL2, NULL, SCENARIO_UNUSED, FUNCTION_NONE}
 133 #endif
 134 };
 135
 136 const serialPortConstraint_t serialPortConstraints[SERIAL_PORT_COUNT] = {
 137     {SERIAL_PORT_USART1,        9600, 250000,   SPF_NONE | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 138     {SERIAL_PORT_USART2,        9600, 250000,   SPF_SUPPORTS_CALLBACK | SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_BIDIR_MODE},
 139 #if (SERIAL_PORT_COUNT > 2)
 140     {SERIAL_PORT_SOFTSERIAL1,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE},
 141     {SERIAL_PORT_SOFTSERIAL2,   9600, 19200,    SPF_SUPPORTS_CALLBACK | SPF_IS_SOFTWARE_INVERTABLE}
 142 #endif
 143 };
 144 #endif
 145 #endif
 146
 147 const functionConstraint_t functionConstraints[] = {
 148         { FUNCTION_CLI,                 9600,  115200, NO_AUTOBAUD, SPF_NONE },
 149         { FUNCTION_GPS,                 9600,  115200, AUTOBAUD,    SPF_NONE },
 150         { FUNCTION_GPS_PASSTHROUGH,     9600,  115200, NO_AUTOBAUD, SPF_NONE },
 151         { FUNCTION_MSP,                 9600,  115200, NO_AUTOBAUD, SPF_NONE },
 152         { FUNCTION_SERIAL_RX,           9600,  250000, NO_AUTOBAUD, SPF_SUPPORTS_SBUS_MODE | SPF_SUPPORTS_CALLBACK },
 153         { FUNCTION_TELEMETRY,           9600,  19200,  NO_AUTOBAUD, SPF_NONE },
 154         { FUNCTION_SMARTPORT_TELEMETRY, 57600, 57600,  NO_AUTOBAUD, SPF_SUPPORTS_BIDIR_MODE },
 155         { FUNCTION_BLACKBOX,            115200,115200, NO_AUTOBAUD, SPF_NONE }
 156 };
 157
 158 #define FUNCTION_CONSTRAINT_COUNT (sizeof(functionConstraints) / sizeof(functionConstraint_t))
 159
 160 typedef struct serialPortSearchResult_s {
 161     serialPortIndex_e portIndex;
 162     serialPortFunction_t *portFunction;
 163     const serialPortConstraint_t *portConstraint;
 164     const functionConstraint_t *functionConstraint;
 165
 166     // private
 167     uint8_t startSerialPortFunctionIndex; // used by findNextSerialPort
 168 } serialPortSearchResult_t;
 169
 170 static const serialPortFunctionList_t serialPortFunctionList = {
 171         SERIAL_PORT_COUNT,
 172         serialPortFunctions
 173 };
 174
 175 const serialPortFunctionList_t *getSerialPortFunctionList(void)
 176 {
 177     return &serialPortFunctionList;
 178 }
 179
 180 uint8_t lookupScenarioIndex(serialPortFunctionScenario_e scenario) {
 181     uint8_t index;
 182     for (index = 0; index < SERIAL_PORT_SCENARIO_COUNT; index++) {
 183         if (serialPortScenarios[index] == scenario) {
 184             break;
 185         }
 186     }
 187     return index;
 188 }
 189
 190 static serialPortIndex_e lookupSerialPortIndexByIdentifier(serialPortIdentifier_e identifier)
 191 {
 192     serialPortIndex_e portIndex;
 193     for (portIndex = 0; portIndex < SERIAL_PORT_COUNT; portIndex++) {
 194         if (serialPortConstraints[portIndex].identifier == identifier) {
 195             break;
 196         }
 197     }
 198     return portIndex;
 199 }
 200
 201 #define IDENTIFIER_NOT_FOUND 0xFF
 202
 203 static uint8_t lookupSerialPortFunctionIndexByIdentifier(serialPortIdentifier_e identifier)
 204 {
 205     uint8_t index;
 206     for (index = 0; index < SERIAL_PORT_COUNT; index++) {
 207         if (serialPortFunctions[index].identifier == identifier) {
 208             return index;
 209         }
 210     }
 211     return IDENTIFIER_NOT_FOUND;
 212 }
 213
 214 static const functionConstraint_t *findFunctionConstraint(serialPortFunction_e function)
 215 {
 216     const functionConstraint_t *functionConstraint = NULL;
 217     uint8_t functionConstraintIndex;
 218
 219     for (functionConstraintIndex = 0; functionConstraintIndex < FUNCTION_CONSTRAINT_COUNT; functionConstraintIndex++) {
 220         functionConstraint = &functionConstraints[functionConstraintIndex];
 221         if (functionConstraint->function == function) {
 222             return functionConstraint;
 223         }
 224     }
 225     return NULL;
 226 }
 227
 228 static uint8_t countBits_uint32(uint32_t n) {
 229   uint8_t c; // c accumulates the total bits set in n
 230   for (c = 0; n; c++)
 231     n &= n - 1; // clear the least significant bit set
 232   return c;
 233 }
 234 static int serialPortFunctionMostSpecificFirstComparator(const void *aPtr, const void *bPtr)
 235 {
 236     serialPortFunction_t *a = (serialPortFunction_t *)aPtr;
 237     serialPortFunction_t *b = (serialPortFunction_t *)bPtr;
 238
 239     return countBits_uint32(a->scenario) - countBits_uint32(b->scenario);
 240 }
 241
 242 static void sortSerialPortFunctions(serialPortFunction_t *serialPortFunctions, uint8_t elements)
 243 {
 244     serialPortFunction_t swap;
 245
 246     int index1;
 247     int index2;
 248
 249     // bubble-sort array (TODO - port selection can be implemented as repeated minimum search with bitmask marking used elements)
 250     for (index1 = 0; index1 < (elements - 1); index1++) {
 251         for (index2 = 0; index2 < elements - index1 - 1; index2++) {
 252             if(serialPortFunctionMostSpecificFirstComparator(&serialPortFunctions[index2], &serialPortFunctions[index2 + 1]) > 0) {
 253                 swap = serialPortFunctions[index2];
 254                 serialPortFunctions[index2] = serialPortFunctions[index2 + 1];
 255                 serialPortFunctions[index2 + 1] = swap;
 256             }
 257         }
 258     }
 259 }
 260
 261 serialPortSearchResult_t *findNextSerialPort(serialPortFunction_e function, const functionConstraint_t *functionConstraint, serialPortSearchResult_t *resultBuffer)
 262 {
 263     uint8_t serialPortFunctionIndex;
 264     serialPortFunction_t *serialPortFunction;
 265     for (serialPortFunctionIndex = resultBuffer->startSerialPortFunctionIndex; serialPortFunctionIndex < SERIAL_PORT_COUNT; serialPortFunctionIndex++) {
 266         serialPortFunction = &serialPortFunctions[serialPortFunctionIndex];
 267
 268         if (!(serialPortFunction->scenario & function)) {
 269             continue;
 270         }
 271
 272         uint8_t serialPortIndex = lookupSerialPortIndexByIdentifier(serialPortFunction->identifier);
 273         const serialPortConstraint_t *serialPortConstraint = &serialPortConstraints[serialPortIndex];
 274
 275 #if defined(CC3D)
 276         if (!feature(FEATURE_SOFTSERIAL) && (
 277                 serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1)) {
 278             continue;
 279         }
 280 #else
 281 #if defined(USE_SOFTSERIAL1) ||(defined(USE_SOFTSERIAL2))
 282         if (!feature(FEATURE_SOFTSERIAL) && (
 283                 serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL1 ||
 284                 serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2
 285         )) {
 286             continue;
 287         }
 288 #endif
 289 #if (defined(NAZE) || defined(OLIMEXINO)) && defined(SONAR)
 290         if (feature(FEATURE_SONAR) && !feature(FEATURE_RX_PARALLEL_PWM) && (serialPortConstraint->identifier == SERIAL_PORT_SOFTSERIAL2)) {
 291             continue;
 292         }
 293 #endif
 294 #endif
 295
 296         if ((serialPortConstraint->feature & functionConstraint->requiredSerialPortFeatures) != functionConstraint->requiredSerialPortFeatures) {
 297             continue;
 298         }
 299
 300         if (functionConstraint->minBaudRate < serialPortConstraint->minBaudRate || functionConstraint->maxBaudRate > serialPortConstraint->maxBaudRate) {
 301             continue;
 302         }
 303
 304         resultBuffer->portIndex = serialPortIndex;
 305         resultBuffer->portConstraint = serialPortConstraint;
 306         resultBuffer->portFunction = serialPortFunction;
 307         resultBuffer->functionConstraint = functionConstraint;
 308
 309         uint8_t nextStartIndex = serialPortFunctionIndex + 1;
 310         resultBuffer->startSerialPortFunctionIndex = nextStartIndex;
 311
 312         return resultBuffer;
 313     }
 314
 315     return NULL;
 316 }
 317
 318 /*
 319  * since this method, and other methods that use it, use a single instance of
 320  * searchPortSearchResult be sure to copy the data out of it before it gets overwritten by another caller.
 321  * If this becomes a problem perhaps change the implementation to use a destination argument.
 322  */
 323 static serialPortSearchResult_t *findSerialPort(serialPortFunction_e function, const functionConstraint_t *functionConstraint)
 324 {
 325     static serialPortSearchResult_t serialPortSearchResult;
 326
 327     memset(&serialPortSearchResult, 0, sizeof(serialPortSearchResult));
 328
 329     // FIXME this only needs to be done once, after the config has been loaded.
 330     sortSerialPortFunctions(serialPortFunctions, SERIAL_PORT_COUNT);
 331
 332     return findNextSerialPort(function, functionConstraint, &serialPortSearchResult);
 333 }
 334
 335
 336 static serialPortFunction_t *findSerialPortFunction(uint16_t functionMask)
 337 {
 338     serialPortIndex_e portIndex;
 339
 340     // find exact match first
 341     for (portIndex = 0; portIndex < SERIAL_PORT_COUNT; portIndex++) {
 342         serialPortFunction_t *serialPortFunction = &serialPortFunctions[portIndex];
 343         if (serialPortFunction->scenario == functionMask) {
 344             return serialPortFunction;
 345         }
 346     }
 347
 348     // find the first port that supports the function requested
 349     for (portIndex = 0; portIndex < SERIAL_PORT_COUNT; portIndex++) {
 350         serialPortFunction_t *serialPortFunction = &serialPortFunctions[portIndex];
 351         if (serialPortFunction->scenario & functionMask) {
 352             return serialPortFunction;
 353         }
 354     }
 355
 356     return NULL;
 357 }
 358
 359 /*
 360  * find a serial port that is:
 361  * a) open
 362  * b) matches the function mask exactly, or if an exact match is not found the first port that supports the function
 363  */
 364 serialPort_t *findOpenSerialPort(uint16_t functionMask)
 365 {
 366     serialPortFunction_t *function = findSerialPortFunction(functionMask);
 367     if (!function) {
 368         return NULL;
 369     }
 370     return function->port;
 371 }
 372
 373
 374
 375 static serialPortFunction_t * findSerialPortFunctionByPort(serialPort_t *port)
 376 {
 377     serialPortFunction_t *serialPortFunction;
 378     uint8_t functionIndex;
 379
 380     for (functionIndex = 0; functionIndex < SERIAL_PORT_COUNT; functionIndex++) {
 381         serialPortFunction = &serialPortFunctions[functionIndex];
 382         if (serialPortFunction->port == port) {
 383             return serialPortFunction;
 384         }
 385     }
 386
 387     return NULL;
 388 }
 389
 390 void beginSerialPortFunction(serialPort_t *port, serialPortFunction_e function)
 391 {
 392     serialPortFunction_t *serialPortFunction = findSerialPortFunctionByPort(port);
 393
 394     serialPortFunction->currentFunction = function;
 395 }
 396
 397 void endSerialPortFunction(serialPort_t *port, serialPortFunction_e function)
 398 {
 399     UNUSED(function);
 400     serialPortFunction_t *serialPortFunction = findSerialPortFunctionByPort(port);
 401
 402     serialPortFunction->currentFunction = FUNCTION_NONE;
 403     serialPortFunction->port = NULL;
 404 }
 405
 406 functionConstraint_t *getConfiguredFunctionConstraint(serialPortFunction_e function)
 407 {
 408     static functionConstraint_t configuredFunctionConstraint;
 409     const functionConstraint_t *functionConstraint;
 410
 411     functionConstraint = findFunctionConstraint(function);
 412     if (!functionConstraint) {
 413         return NULL;
 414     }
 415     memcpy(&configuredFunctionConstraint, functionConstraint, sizeof(functionConstraint_t));
 416
 417     switch(function) {
 418         case FUNCTION_MSP:
 419             configuredFunctionConstraint.maxBaudRate = serialConfig->msp_baudrate;
 420             break;
 421
 422         case FUNCTION_CLI:
 423             configuredFunctionConstraint.minBaudRate = serialConfig->cli_baudrate;
 424             configuredFunctionConstraint.maxBaudRate = configuredFunctionConstraint.minBaudRate;
 425             break;
 426
 427         case FUNCTION_GPS_PASSTHROUGH:
 428             configuredFunctionConstraint.minBaudRate = serialConfig->gps_passthrough_baudrate;
 429             configuredFunctionConstraint.maxBaudRate = configuredFunctionConstraint.minBaudRate;
 430             break;
 431
 432 #ifdef TELEMETRY
 433         case FUNCTION_TELEMETRY:
 434         case FUNCTION_SMARTPORT_TELEMETRY:
 435             configuredFunctionConstraint.minBaudRate = getTelemetryProviderBaudRate();
 436             configuredFunctionConstraint.maxBaudRate = configuredFunctionConstraint.minBaudRate;
 437             break;
 438 #endif
 439 #ifdef SERIAL_RX
 440         case FUNCTION_SERIAL_RX:
 441             updateSerialRxFunctionConstraint(&configuredFunctionConstraint);
 442             break;
 443 #endif
 444         case FUNCTION_GPS:
 445             configuredFunctionConstraint.maxBaudRate = serialConfig->gps_baudrate;
 446             break;
 447         default:
 448             break;
 449     }
 450     return &configuredFunctionConstraint;
 451 }
 452
 453 bool isSerialConfigValid(serialConfig_t *serialConfigToCheck)
 454 {
 455     serialPortSearchResult_t *searchResult;
 456     functionConstraint_t *functionConstraint;
 457
 458     serialConfig = serialConfigToCheck;
 459
 460     functionConstraint = getConfiguredFunctionConstraint(FUNCTION_MSP);
 461     searchResult = findSerialPort(FUNCTION_MSP, functionConstraint);
 462     if (!searchResult) {
 463         return false;
 464     }
 465
 466     functionConstraint = getConfiguredFunctionConstraint(FUNCTION_CLI);
 467     searchResult = findSerialPort(FUNCTION_CLI, functionConstraint);
 468     if (!searchResult) {
 469         return false;
 470     }
 471
 472     functionConstraint = getConfiguredFunctionConstraint(FUNCTION_GPS);
 473     searchResult = findSerialPort(FUNCTION_GPS, functionConstraint);
 474     if (feature(FEATURE_GPS) && !searchResult) {
 475         return false;
 476     }
 477
 478     functionConstraint = getConfiguredFunctionConstraint(FUNCTION_SERIAL_RX);
 479     searchResult = findSerialPort(FUNCTION_SERIAL_RX, functionConstraint);
 480     if (feature(FEATURE_RX_SERIAL) && !searchResult) {
 481         return false;
 482     }
 483
 484     functionConstraint = getConfiguredFunctionConstraint(FUNCTION_TELEMETRY);
 485     searchResult = findSerialPort(FUNCTION_TELEMETRY, functionConstraint);
 486     // TODO check explicitly for SmartPort config
 487     if (!searchResult) {
 488         functionConstraint = getConfiguredFunctionConstraint(FUNCTION_SMARTPORT_TELEMETRY);
 489         searchResult = findSerialPort(FUNCTION_SMARTPORT_TELEMETRY, functionConstraint);
 490     }
 491     if (feature(FEATURE_TELEMETRY) && !searchResult) {
 492         return false;
 493     }
 494
 495     uint8_t functionIndex;
 496     uint8_t cliPortCount = 0;
 497     uint8_t mspPortCount = 0;
 498     for (functionIndex = 0; functionIndex < SERIAL_PORT_COUNT; functionIndex++) {
 499         if (serialPortFunctions[functionIndex].scenario & FUNCTION_CLI) {
 500             if (++cliPortCount > 1) {
 501                 return false;
 502             }
 503         }
 504         if (serialPortFunctions[functionIndex].scenario & FUNCTION_MSP) {
 505             if (++mspPortCount > MAX_MSP_PORT_COUNT) {
 506                 return false;
 507             }
 508         }
 509     }
 510     return true;
 511 }
 512
 513 bool doesConfigurationUsePort(serialPortIdentifier_e portIdentifier)
 514 {
 515     serialPortSearchResult_t *searchResult;
 516     const functionConstraint_t *functionConstraint;
 517     serialPortFunction_e function;
 518
 519     uint8_t index;
 520     for (index = 0; index < FUNCTION_CONSTRAINT_COUNT; index++) {
 521         function = functionConstraints[index].function;
 522         functionConstraint = findFunctionConstraint(function);
 523         searchResult = findSerialPort(function, functionConstraint);
 524         if (searchResult->portConstraint->identifier == portIdentifier) {
 525             return true;
 526         }
 527     }
 528     return false;
 529 }
 530
 531 bool canOpenSerialPort(serialPortFunction_e function)
 532 {
 533     functionConstraint_t *functionConstraint = getConfiguredFunctionConstraint(function);
 534
 535     serialPortSearchResult_t *result = findSerialPort(function, functionConstraint);
 536     return result != NULL;
 537 }
 538
 539 bool isSerialPortFunctionShared(serialPortFunction_e functionToUse, uint16_t functionMask)
 540 {
 541     functionConstraint_t *functionConstraint = getConfiguredFunctionConstraint(functionToUse);
 542     serialPortSearchResult_t *result = findSerialPort(functionToUse, functionConstraint);
 543     if (!result) {
 544         return false;
 545     }
 546
 547     return result->portFunction->scenario & functionMask;
 548 }
 549
 550 serialPort_t *findSharedSerialPort(serialPortFunction_e functionToUse, uint16_t functionMask)
 551 {
 552     functionConstraint_t *functionConstraint = getConfiguredFunctionConstraint(functionToUse);
 553     serialPortSearchResult_t *result = findSerialPort(functionToUse, functionConstraint);
 554
 555     if (result->portFunction->scenario & functionMask) {
 556         return result->portFunction->port;
 557     }
 558     return NULL;
 559 }
 560
 561 void applySerialConfigToPortFunctions(serialConfig_t *serialConfig)
 562 {
 563     uint32_t portIndex = 0, serialPortIdentifier, constraintIndex;
 564
 565     for (constraintIndex = 0; constraintIndex < SERIAL_PORT_COUNT && portIndex < SERIAL_PORT_COUNT; constraintIndex++) {
 566         serialPortIdentifier = serialPortConstraints[constraintIndex].identifier;
 567         uint32_t functionIndex = lookupSerialPortFunctionIndexByIdentifier(serialPortIdentifier);
 568         if (functionIndex == IDENTIFIER_NOT_FOUND) {
 569             continue;
 570         }
 571         serialPortFunctions[functionIndex].scenario = serialPortScenarios[serialConfig->serial_port_scenario[portIndex++]];
 572     }
 573 }
 574
 575 serialPort_t *openSerialPort(serialPortFunction_e function, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion)
 576 {
 577     serialPort_t *serialPort = NULL;
 578
 579     functionConstraint_t *initialFunctionConstraint = getConfiguredFunctionConstraint(function);
 580
 581     functionConstraint_t updatedFunctionConstraint;
 582     memcpy(&updatedFunctionConstraint, initialFunctionConstraint, sizeof(updatedFunctionConstraint));
 583     if (initialFunctionConstraint->autoBaud == NO_AUTOBAUD) {
 584         updatedFunctionConstraint.minBaudRate = baudRate;
 585         updatedFunctionConstraint.maxBaudRate = baudRate;
 586     }
 587     functionConstraint_t *functionConstraint = &updatedFunctionConstraint;
 588
 589     serialPortSearchResult_t *searchResult = findSerialPort(function, functionConstraint);
 590
 591     while(searchResult && searchResult->portFunction->port) {
 592         // port is already open, find the next one
 593         searchResult = findNextSerialPort(function, functionConstraint, searchResult);
 594     }
 595
 596     if (!searchResult) {
 597         return NULL;
 598     }
 599
 600     serialPortIndex_e portIndex = searchResult->portIndex;
 601
 602     const serialPortConstraint_t *serialPortConstraint = searchResult->portConstraint;
 603
 604     serialPortIdentifier_e identifier = serialPortConstraint->identifier;
 605     switch(identifier) {
 606 #ifdef USE_VCP
 607         case SERIAL_PORT_USB_VCP:
 608             serialPort = usbVcpOpen();
 609             break;
 610 #endif
 611 #ifdef USE_USART1
 612         case SERIAL_PORT_USART1:
 613             serialPort = uartOpen(USART1, callback, baudRate, mode, inversion);
 614             break;
 615 #endif
 616 #ifdef USE_USART2
 617         case SERIAL_PORT_USART2:
 618             serialPort = uartOpen(USART2, callback, baudRate, mode, inversion);
 619             break;
 620 #endif
 621 #ifdef USE_USART3
 622         case SERIAL_PORT_USART3:
 623             serialPort = uartOpen(USART3, callback, baudRate, mode, inversion);
 624             break;
 625 #endif
 626 #ifdef USE_SOFTSERIAL1
 627         case SERIAL_PORT_SOFTSERIAL1:
 628             serialPort = openSoftSerial(SOFTSERIAL1, callback, baudRate, inversion);
 629             serialSetMode(serialPort, mode);
 630             break;
 631 #endif
 632 #ifdef USE_SOFTSERIAL2
 633         case SERIAL_PORT_SOFTSERIAL2:
 634             serialPort = openSoftSerial(SOFTSERIAL2, callback, baudRate, inversion);
 635             serialSetMode(serialPort, mode);
 636             break;
 637 #endif
 638         default:
 639             break;
 640     }
 641
 642     if (!serialPort) {
 643         return NULL;
 644     }
 645
 646     serialPort->identifier = identifier;
 647
 648     serialPorts[portIndex] = serialPort;
 649
 650     serialPortFunction_t *serialPortFunction = searchResult->portFunction;
 651
 652     serialPortFunction->port = serialPort;
 653     //serialPortFunction->identifier = identifier;
 654
 655     beginSerialPortFunction(serialPort, function);
 656
 657     return serialPort;
 658 }
 659
 660 void serialInit(serialConfig_t *initialSerialConfig)
 661 {
 662     serialConfig = initialSerialConfig;
 663     applySerialConfigToPortFunctions(serialConfig);
 664
 665 #ifdef TELEMETRY
 666     if (telemetryAllowsOtherSerial(FUNCTION_MSP))
 667 #endif
 668         mspInit(serialConfig);
 669
 670 #ifdef TELEMETRY
 671     if (telemetryAllowsOtherSerial(FUNCTION_CLI))
 672 #endif
 673         cliInit(serialConfig);
 674 }
 675
 676 void handleSerial(void)
 677 {
 678     // in cli mode, all serial stuff goes to here. enter cli mode by sending #
 679     if (cliMode) {
 680         cliProcess();
 681         return;
 682     }
 683
 684 #ifdef TELEMETRY
 685     if (telemetryAllowsOtherSerial(FUNCTION_MSP))
 686 #endif
 687         mspProcess();
 688 }
 689
 690 void waitForSerialPortToFinishTransmitting(serialPort_t *serialPort)
 691 {
 692     while (!isSerialTransmitBufferEmpty(serialPort)) {
 693         delay(10);
 694     };
 695 }
 696
 697 // evaluate all other incoming serial data
 698 void evaluateOtherData(uint8_t sr)
 699 {
 700     if (sr == '#')
 701         cliProcess();
 702     else if (sr == serialConfig->reboot_character)
 703         systemResetToBootloader();
 704 }
 705