src/main/rx/fport2.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV 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  * INAV is distributed in the hope that it
  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 <stdlib.h>
  24 #include <string.h>
  25
  26 #include "platform.h"
  27
  28 #ifdef USE_SERIALRX_FPORT2
  29
  30 #include "build/debug.h"
  31
  32 #include "common/log.h"
  33 #include "common/maths.h"
  34 #include "common/utils.h"
  35
  36 #include "drivers/time.h"
  37
  38 #ifdef MSP_FIRMWARE_UPDATE
  39 #include "fc/firmware_update.h"
  40 #endif
  41
  42 #include "io/serial.h"
  43 #include "io/smartport_master.h"
  44
  45 #ifdef USE_TELEMETRY
  46 #include "telemetry/telemetry.h"
  47 #include "telemetry/smartport.h"
  48 #endif
  49
  50 #include "rx/frsky_crc.h"
  51 #include "rx/rx.h"
  52 #include "rx/sbus_channels.h"
  53 #include "rx/fport2.h"
  54
  55
  56 #define FPORT2_MIN_TELEMETRY_RESPONSE_DELAY_US 500
  57 #define FPORT2_MAX_TELEMETRY_RESPONSE_DELAY_US 3000
  58 #define FPORT2_OTA_MAX_RESPONSE_TIME_US_DEFAULT 200
  59 #define FPORT2_OTA_MIN_RESPONSE_DELAY_US_DEFAULT 50
  60 #define FPORT2_MAX_TELEMETRY_AGE_MS 500
  61 #define FPORT2_FC_COMMON_ID 0x1B
  62 #define FPORT2_FC_MSP_ID 0x0D
  63 #define FPORT2_BAUDRATE 115200
  64 #define FBUS_BAUDRATE 460800
  65 #define FPORT2_PORT_OPTIONS (SERIAL_STOPBITS_1 | SERIAL_PARITY_NO)
  66 #define FPORT2_RX_TIMEOUT 120 // µs
  67 #define FPORT2_CONTROL_FRAME_LENGTH 24
  68 #define FPORT2_OTA_DATA_FRAME_LENGTH 32
  69 #define FPORT2_DOWNLINK_FRAME_LENGTH 8
  70 #define FPORT2_UPLINK_FRAME_LENGTH 8
  71 #define FPORT2_TELEMETRY_MAX_CONSECUTIVE_TELEMETRY_FRAMES 2
  72 #define FPORT2_OTA_DATA_FRAME_BYTES 32
  73
  74 enum {
  75     DEBUG_FPORT2_FRAME_INTERVAL = 0,
  76     DEBUG_FPORT2_FRAME_ERRORS,
  77     DEBUG_FPORT2_FRAME_LAST_ERROR,
  78     DEBUG_FPORT2_TELEMETRY_INTERVAL,
  79     DEBUG_FPORT2_MAX_BUFFER_USAGE,
  80     DEBUG_FPORT2_OTA_FRAME_RESPONSE_TIME,
  81     DEBUG_FPORT2_OTA_RECEIVED_BYTES,
  82 };
  83
  84 enum {
  85     DEBUG_FPORT2_NO_ERROR = 0,
  86     DEBUG_FPORT2_ERROR_TIMEOUT,
  87     DEBUG_FPORT2_ERROR_OVERSIZE,
  88     DEBUG_FPORT2_ERROR_SIZE,
  89     DEBUG_FPORT2_ERROR_CHECKSUM,
  90     DEBUG_FPORT2_ERROR_PHYID_CRC,
  91     DEBUG_FPORT2_ERROR_TYPE,
  92     DEBUG_FPORT2_ERROR_TYPE_SIZE,
  93     DEBUG_FPORT2_ERROR_OTA_BAD_ADDRESS,
  94 };
  95
  96 typedef enum {
  97     CFT_RC = 0xFF,
  98     CFT_OTA_START = 0xF0,
  99     CFT_OTA_DATA = 0xF1,
 100     CFT_OTA_STOP = 0xF2
 101 } fport2_control_frame_type_e;
 102
 103 typedef enum {
 104     FT_CONTROL,
 105     FT_DOWNLINK
 106 } frame_type_e;
 107
 108 typedef enum {
 109     FS_CONTROL_FRAME_START,
 110     FS_CONTROL_FRAME_TYPE,
 111     FS_CONTROL_FRAME_DATA,
 112     FS_DOWNLINK_FRAME_START,
 113     FS_DOWNLINK_FRAME_DATA
 114 } frame_state_e;
 115
 116 enum {
 117     FPORT2_FRAME_ID_NULL = 0x00,
 118     FPORT2_FRAME_ID_DATA = 0x10,
 119     FPORT2_FRAME_ID_READ = 0x30,
 120     FPORT2_FRAME_ID_WRITE = 0x31,
 121     FPORT2_FRAME_ID_RESPONSE = 0x32,
 122     FPORT2_FRAME_ID_OTA_START = 0xF0,
 123     FPORT2_FRAME_ID_OTA_DATA = 0xF1,
 124     FPORT2_FRAME_ID_OTA_STOP = 0xF2
 125 };
 126
 127 typedef struct {
 128     sbusChannels_t channels;
 129     uint8_t rssi;
 130 } rcData_t;
 131
 132 typedef struct {
 133     uint8_t phyID;
 134     /*uint8_t phyID : 5;*/
 135     /*uint8_t phyXOR : 3;*/
 136     smartPortPayload_t telemetryData;
 137 } PACKED fportDownlinkData_t;
 138
 139 typedef struct {
 140     uint8_t type;
 141     union {
 142         rcData_t rc;
 143         uint8_t ota[FPORT2_OTA_DATA_FRAME_BYTES];
 144     };
 145 } PACKED fportControlFrame_t;
 146
 147 typedef struct {
 148     uint8_t type;
 149     union {
 150         fportControlFrame_t control;
 151         fportDownlinkData_t downlink;
 152     };
 153 } fportFrame_t;
 154
 155 // RX frames ring buffer
 156 #define NUM_RX_BUFFERS 15
 157
 158 typedef struct fportBuffer_s {
 159     uint8_t data[sizeof(fportFrame_t)+1]; // +1 for CRC
 160     uint8_t length;
 161 } fportBuffer_t;
 162
 163 typedef struct {
 164     uint32_t size;
 165     uint8_t  crc;
 166 } PACKED firmwareUpdateHeader_t;
 167
 168 static volatile fportBuffer_t rxBuffer[NUM_RX_BUFFERS];
 169 static volatile uint8_t rxBufferWriteIndex = 0;
 170 static volatile uint8_t rxBufferReadIndex = 0;
 171
 172 static serialPort_t *fportPort;
 173
 174 #ifdef USE_TELEMETRY_SMARTPORT
 175 static smartPortPayload_t *mspPayload = NULL;
 176 static bool telemetryEnabled = false;
 177 static volatile timeUs_t lastTelemetryFrameReceivedUs;
 178 static volatile bool clearToSend = false;
 179 static volatile bool sendNullFrame = false;
 180 static uint8_t downlinkPhyID;
 181 static const smartPortPayload_t emptySmartPortFrame = { .frameId = 0, .valueId = 0, .data = 0 };
 182 static smartPortPayload_t *otaResponsePayload = NULL;
 183 static bool otaMode = false;
 184 static bool otaDataNeedsProcessing = false;
 185 static uint16_t otaMinResponseDelay = FPORT2_OTA_MIN_RESPONSE_DELAY_US_DEFAULT;
 186 static uint16_t otaMaxResponseTime = FPORT2_OTA_MAX_RESPONSE_TIME_US_DEFAULT;
 187 static uint32_t otaDataAddress;
 188 static uint8_t otaDataBuffer[FPORT2_OTA_DATA_FRAME_BYTES];
 189 static timeUs_t otaFrameEndTimestamp = 0;
 190 static bool firmwareUpdateError = false;
 191 #ifdef MSP_FIRMWARE_UPDATE
 192 static uint8_t firmwareUpdateCRC;
 193 static timeUs_t readyToUpdateFirmwareTimestamp = 0;
 194 #endif
 195 #endif
 196
 197 static volatile uint16_t frameErrors = 0;
 198
 199 static void reportFrameError(uint8_t errorReason) {
 200     UNUSED(errorReason);
 201     frameErrors++;
 202 }
 203
 204 static void clearWriteBuffer(void)
 205 {
 206     rxBuffer[rxBufferWriteIndex].length = 0;
 207 }
 208
 209 static bool nextWriteBuffer(void)
 210 {
 211     const uint8_t nextWriteIndex = (rxBufferWriteIndex + 1) % NUM_RX_BUFFERS;
 212     if (nextWriteIndex != rxBufferReadIndex) {
 213         rxBufferWriteIndex = nextWriteIndex;
 214         clearWriteBuffer();
 215         return true;
 216     } else {
 217         clearWriteBuffer();
 218         return false;
 219     }
 220 }
 221
 222 static uint8_t writeBuffer(uint8_t byte)
 223 {
 224     volatile uint8_t * const buffer = rxBuffer[rxBufferWriteIndex].data;
 225     volatile uint8_t * const buflen = &rxBuffer[rxBufferWriteIndex].length;
 226     buffer[*buflen] = byte;
 227     *buflen += 1;
 228     return *buflen;
 229 }
 230
 231 // UART RX ISR
 232 static void fportDataReceive(uint16_t byte, void *callback_data)
 233 {
 234     UNUSED(callback_data);
 235
 236     static volatile frame_state_e state = FS_CONTROL_FRAME_START;
 237     static volatile timeUs_t lastRxByteTimestamp = 0;
 238     const timeUs_t currentTimeUs = micros();
 239     const timeUs_t timeSincePreviousRxByte = lastRxByteTimestamp ? currentTimeUs - lastRxByteTimestamp : 0;
 240     static unsigned controlFrameSize;
 241
 242     lastRxByteTimestamp = currentTimeUs;
 243 #if defined(USE_TELEMETRY_SMARTPORT)
 244     clearToSend = false;
 245 #endif
 246
 247     if ((state != FS_CONTROL_FRAME_START) && (timeSincePreviousRxByte > FPORT2_RX_TIMEOUT)) {
 248         state = FS_CONTROL_FRAME_START;
 249     }
 250
 251     switch (state) {
 252
 253         case FS_CONTROL_FRAME_START:
 254             if ((byte == FPORT2_CONTROL_FRAME_LENGTH) || (byte == FPORT2_OTA_DATA_FRAME_LENGTH)) {
 255                 clearWriteBuffer();
 256                 writeBuffer(FT_CONTROL);
 257                 state = FS_CONTROL_FRAME_TYPE;
 258             }
 259             break;
 260
 261         case FS_CONTROL_FRAME_TYPE:
 262             if ((byte == CFT_RC) || ((byte >= CFT_OTA_START) && (byte <= CFT_OTA_STOP))) {
 263                 unsigned controlFrameType = byte;
 264                 writeBuffer(controlFrameType);
 265                 controlFrameSize = (controlFrameType == CFT_OTA_DATA ? FPORT2_OTA_DATA_FRAME_LENGTH : FPORT2_CONTROL_FRAME_LENGTH) + 2; // +2 = General frame type + CRC
 266                 state = FS_CONTROL_FRAME_DATA;
 267             } else {
 268                 state = FS_CONTROL_FRAME_START;
 269             }
 270             break;
 271
 272         case FS_CONTROL_FRAME_DATA: {
 273             if (writeBuffer(byte) > controlFrameSize) {
 274                 nextWriteBuffer();
 275                 state = FS_DOWNLINK_FRAME_START;
 276             }
 277             break;
 278         }
 279
 280         case FS_DOWNLINK_FRAME_START:
 281             if (byte == FPORT2_DOWNLINK_FRAME_LENGTH) {
 282                 writeBuffer(FT_DOWNLINK);
 283                 state = FS_DOWNLINK_FRAME_DATA;
 284             } else {
 285                 state = FS_CONTROL_FRAME_START;
 286             }
 287             break;
 288
 289         case FS_DOWNLINK_FRAME_DATA:
 290             if (writeBuffer(byte) > (FPORT2_DOWNLINK_FRAME_LENGTH + 1)) {
 291 #if defined(USE_TELEMETRY_SMARTPORT)
 292                 if ((rxBuffer[rxBufferWriteIndex].data[2] >= FPORT2_FRAME_ID_OTA_START) && (rxBuffer[rxBufferWriteIndex].data[2] <= FPORT2_FRAME_ID_OTA_STOP)) {
 293                     otaFrameEndTimestamp = currentTimeUs;
 294                 }
 295                 lastTelemetryFrameReceivedUs = currentTimeUs;
 296 #endif
 297                 nextWriteBuffer();
 298                 state = FS_CONTROL_FRAME_START;
 299             }
 300             break;
 301
 302         default:
 303             state = FS_CONTROL_FRAME_START;
 304             break;
 305
 306     }
 307 }
 308
 309 #if defined(USE_TELEMETRY_SMARTPORT)
 310 static void writeUplinkFramePhyID(uint8_t phyID, const smartPortPayload_t *payload)
 311 {
 312     serialWrite(fportPort, FPORT2_UPLINK_FRAME_LENGTH);
 313     serialWrite(fportPort, phyID);
 314
 315     uint16_t checksum = 0;
 316     frskyCheckSumStep(&checksum, phyID);
 317     uint8_t *data = (uint8_t *)payload;
 318     for (unsigned i = 0; i < sizeof(smartPortPayload_t); ++i, ++data) {
 319         serialWrite(fportPort, *data);
 320         frskyCheckSumStep(&checksum, *data);
 321     }
 322     frskyCheckSumFini(&checksum);
 323     serialWrite(fportPort, checksum);
 324 }
 325
 326 static void writeUplinkFrame(const smartPortPayload_t *payload)
 327 {
 328     writeUplinkFramePhyID(FPORT2_FC_COMMON_ID, payload);
 329 }
 330 #endif
 331
 332 static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig)
 333 {
 334 #ifdef USE_TELEMETRY_SMARTPORT
 335     static smartPortPayload_t payloadBuffer;
 336     static bool hasTelemetryRequest = false;
 337     static uint32_t otaPrevDataAddress;
 338     static bool otaJustStarted = false;
 339     static bool otaGotData = false;
 340 #endif
 341     static timeUs_t frameReceivedTimestamp = 0;
 342
 343     uint8_t result = RX_FRAME_PENDING;
 344     timeUs_t currentTimeUs = micros();
 345
 346     if (rxBufferReadIndex != rxBufferWriteIndex) {
 347
 348         volatile uint8_t *buffer = rxBuffer[rxBufferReadIndex].data;
 349         uint8_t buflen = rxBuffer[rxBufferReadIndex].length;
 350
 351         fportFrame_t *frame = (fportFrame_t *)buffer;
 352
 353         if (!frskyCheckSumIsGood((uint8_t *)buffer + 1, buflen - 1)) {
 354             reportFrameError(DEBUG_FPORT2_ERROR_CHECKSUM);
 355         } else {
 356
 357             switch (frame->type) {
 358
 359                 case FT_CONTROL:
 360                     switch (frame->control.type) {
 361
 362                         case CFT_RC:
 363                             result = sbusChannelsDecode(rxRuntimeConfig, &frame->control.rc.channels);
 364                             lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(frame->control.rc.rssi, 0, 100, 0, RSSI_MAX_VALUE));
 365                             frameReceivedTimestamp = currentTimeUs;
 366 #if defined(USE_TELEMETRY_SMARTPORT)
 367                             otaMode = false;
 368 #endif
 369                             break;
 370
 371 #if defined(USE_TELEMETRY_SMARTPORT)
 372                         case CFT_OTA_START: {
 373                             uint8_t otaMinResponseDelayByte = frame->control.ota[0];
 374                             if ((otaMinResponseDelayByte > 0) && (otaMinResponseDelayByte <= 4)) {
 375                                 otaMinResponseDelay = otaMinResponseDelayByte * 100;
 376                             } else {
 377                                 otaMinResponseDelay = FPORT2_OTA_MIN_RESPONSE_DELAY_US_DEFAULT;
 378                             }
 379
 380                             uint8_t otaMaxResponseTimeByte = frame->control.ota[1];
 381                             if (otaMaxResponseTimeByte > 0) {
 382                                 otaMaxResponseTime = otaMaxResponseTimeByte * 100;
 383                             } else {
 384                                 otaMaxResponseTime = FPORT2_OTA_MAX_RESPONSE_TIME_US_DEFAULT;
 385                             }
 386
 387                             otaMode = true;
 388                             break;
 389                         }
 390
 391                         case CFT_OTA_DATA:
 392                             if (otaMode) {
 393                                 memcpy(otaDataBuffer, frame->control.ota, sizeof(otaDataBuffer));
 394                                 otaGotData = true;
 395                             } else {
 396                                 reportFrameError(DEBUG_FPORT2_ERROR_TYPE);
 397                             }
 398                             break;
 399
 400                         case CFT_OTA_STOP:
 401                             if (!otaMode) {
 402                                 reportFrameError(DEBUG_FPORT2_ERROR_TYPE);
 403                             }
 404                             break;
 405 #endif
 406
 407                         default:
 408                             reportFrameError(DEBUG_FPORT2_ERROR_TYPE);
 409                             break;
 410
 411                     }
 412                     break;
 413
 414                 case FT_DOWNLINK:
 415 #if defined(USE_TELEMETRY_SMARTPORT)
 416                     if (!telemetryEnabled) {
 417                         break;
 418                     }
 419
 420                     downlinkPhyID = frame->downlink.phyID;
 421                     uint8_t frameId = frame->downlink.telemetryData.frameId;
 422
 423                     switch (frameId) {
 424
 425                         case FPORT2_FRAME_ID_NULL:
 426                             hasTelemetryRequest = true;
 427                             sendNullFrame = true;
 428                             break;
 429
 430                         case FPORT2_FRAME_ID_DATA:
 431                             hasTelemetryRequest = true;
 432                             break;
 433
 434                         case FPORT2_FRAME_ID_OTA_START:
 435                         case FPORT2_FRAME_ID_OTA_DATA:
 436                         case FPORT2_FRAME_ID_OTA_STOP:
 437                             switch (frameId) {
 438                                 case FPORT2_FRAME_ID_OTA_START:
 439                                     if (otaMode) {
 440                                         otaJustStarted = true;
 441                                         otaPrevDataAddress = 0;
 442                                         hasTelemetryRequest = true;
 443                                         otaDataNeedsProcessing = false;
 444                                         firmwareUpdateError = false;
 445                                     } else {
 446                                         reportFrameError(DEBUG_FPORT2_ERROR_TYPE);
 447                                     }
 448                                     break;
 449
 450                                 case FPORT2_FRAME_ID_OTA_DATA:
 451                                     if (otaMode) {
 452                                         otaDataAddress = frame->downlink.telemetryData.data;
 453                                         if (otaGotData && (otaDataAddress == (otaJustStarted ? 0 : otaPrevDataAddress + FPORT2_OTA_DATA_FRAME_BYTES))) { // check that we got a control frame with data and check address
 454                                             otaPrevDataAddress = otaDataAddress;
 455                                             otaGotData = false;
 456                                             otaDataNeedsProcessing = true;
 457                                         }
 458                                         hasTelemetryRequest = true;
 459                                         otaJustStarted = false;
 460                                     } else {
 461                                         reportFrameError(DEBUG_FPORT2_ERROR_TYPE);
 462                                     }
 463                                     break;
 464
 465                                 case FPORT2_FRAME_ID_OTA_STOP:
 466                                     if (otaMode) {
 467                                         hasTelemetryRequest = true;
 468                                     } else {
 469                                         reportFrameError(DEBUG_FPORT2_ERROR_TYPE);
 470                                     }
 471                                     break;
 472                             }
 473                             if (hasTelemetryRequest) {
 474                                 memcpy(&payloadBuffer, &frame->downlink.telemetryData, sizeof(payloadBuffer));
 475                                 otaResponsePayload = &payloadBuffer;
 476                             }
 477                             break;
 478
 479                         default:
 480                             if ((frameId == FPORT2_FRAME_ID_READ) && (downlinkPhyID == FPORT2_FC_MSP_ID)) {
 481                                 memcpy(&payloadBuffer, &frame->downlink.telemetryData, sizeof(payloadBuffer));
 482                                 mspPayload = &payloadBuffer;
 483                                 hasTelemetryRequest = true;
 484                             } else if (downlinkPhyID != FPORT2_FC_COMMON_ID) {
 485 #if defined(USE_SMARTPORT_MASTER)
 486                                 int8_t smartportPhyID = smartportMasterStripPhyIDCheckBits(downlinkPhyID);
 487                                 if (smartportPhyID != -1) {
 488                                     smartportMasterForward(smartportPhyID, &frame->downlink.telemetryData);
 489                                     hasTelemetryRequest = true;
 490                                 }
 491 #endif
 492                             }
 493                             break;
 494
 495                     }
 496 #endif
 497                     break;
 498
 499                 default:
 500                     reportFrameError(DEBUG_FPORT2_ERROR_TYPE);
 501                     break;
 502
 503             }
 504
 505         }
 506
 507         rxBufferReadIndex = (rxBufferReadIndex + 1) % NUM_RX_BUFFERS;
 508     }
 509
 510 #if defined(USE_TELEMETRY_SMARTPORT)
 511     if (((mspPayload || hasTelemetryRequest) && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= FPORT2_MIN_TELEMETRY_RESPONSE_DELAY_US)
 512            || (otaResponsePayload && cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) >= otaMinResponseDelay)) {
 513         hasTelemetryRequest = false;
 514         clearToSend = true;
 515         result |= RX_FRAME_PROCESSING_REQUIRED;
 516     }
 517 #endif
 518
 519     if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > MS2US(FPORT2_MAX_TELEMETRY_AGE_MS))) {
 520         lqTrackerSet(rxRuntimeConfig->lqTracker, 0);
 521         frameReceivedTimestamp = 0;
 522     }
 523
 524 #ifdef MSP_FIRMWARE_UPDATE
 525     if (readyToUpdateFirmwareTimestamp && (cmpTimeUs(currentTimeUs, readyToUpdateFirmwareTimestamp) > 2000000)) {
 526         readyToUpdateFirmwareTimestamp = 0;
 527         firmwareUpdateExec(firmwareUpdateCRC);
 528     }
 529 #endif
 530
 531     return result;
 532 }
 533
 534 static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig)
 535 {
 536     UNUSED(rxRuntimeConfig);
 537
 538 #if defined(USE_TELEMETRY_SMARTPORT)
 539
 540     timeUs_t currentTimeUs = micros();
 541     if (cmpTimeUs(currentTimeUs, lastTelemetryFrameReceivedUs) > FPORT2_MAX_TELEMETRY_RESPONSE_DELAY_US) {
 542        clearToSend = false;
 543     }
 544
 545     if (clearToSend) {
 546         if (otaResponsePayload) {
 547             switch (otaResponsePayload->frameId) {
 548
 549                 case FPORT2_FRAME_ID_OTA_DATA: {
 550                     if (otaDataNeedsProcessing && !firmwareUpdateError) { // We have got fresh data
 551 #ifdef MSP_FIRMWARE_UPDATE
 552                         static uint32_t firmwareUpdateSize;
 553                         uint32_t receivedSize = otaDataAddress - FPORT2_OTA_DATA_FRAME_BYTES;
 554                         if (otaDataAddress == 0) {
 555                             static firmwareUpdateHeader_t *header = (firmwareUpdateHeader_t *)otaDataBuffer;
 556                             firmwareUpdateSize = header->size;
 557                             firmwareUpdateCRC = header->crc;
 558                             firmwareUpdateError = !firmwareUpdatePrepare(firmwareUpdateSize);
 559                         } else if (receivedSize < firmwareUpdateSize) {
 560                             uint8_t firmwareDataSize = MIN((uint8_t)FPORT2_OTA_DATA_FRAME_BYTES, firmwareUpdateSize - receivedSize);
 561                             firmwareUpdateError = !firmwareUpdateStore(otaDataBuffer, firmwareDataSize);
 562                         }
 563 #else
 564                         firmwareUpdateError = true;
 565 #endif
 566                     }
 567                     otaDataNeedsProcessing = false;
 568                     break;
 569                 }
 570
 571                 case FPORT2_FRAME_ID_OTA_STOP:
 572                     otaMode = false;
 573 #ifdef MSP_FIRMWARE_UPDATE
 574                     readyToUpdateFirmwareTimestamp = currentTimeUs;
 575 #endif
 576                     break;
 577
 578             }
 579
 580             timeDelta_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp);
 581             if (!firmwareUpdateError && (otaResponseTime <= otaMaxResponseTime)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash)
 582                 writeUplinkFramePhyID(downlinkPhyID, otaResponsePayload);
 583             }
 584
 585             otaResponsePayload = NULL;
 586             clearToSend = false;
 587
 588         } else if ((downlinkPhyID == FPORT2_FC_COMMON_ID) || (downlinkPhyID == FPORT2_FC_MSP_ID)) {
 589             if ((downlinkPhyID == FPORT2_FC_MSP_ID) && !mspPayload) {
 590                 clearToSend = false;
 591             } else if (!sendNullFrame) {
 592                 processSmartPortTelemetry(mspPayload, &clearToSend, NULL);
 593                 mspPayload = NULL;
 594             }
 595
 596         } else {
 597 #if defined(USE_SMARTPORT_MASTER)
 598             int8_t smartportPhyID = smartportMasterStripPhyIDCheckBits(downlinkPhyID);
 599
 600             if (smartportPhyID != -1) {
 601                 if (sendNullFrame) {
 602                     if (!smartportMasterPhyIDIsActive(smartportPhyID)) { // send null frame only if the sensor is active
 603                         clearToSend = false;
 604                     }
 605                 } else {
 606                     smartPortPayload_t forwardPayload;
 607                     if (smartportMasterNextForwardResponse(smartportPhyID, &forwardPayload) || smartportMasterGetSensorPayload(smartportPhyID, &forwardPayload)) {
 608                         writeUplinkFramePhyID(downlinkPhyID, &forwardPayload);
 609                     }
 610                     clearToSend = false; // either we answered or the sensor is not active, do not send null frame
 611                 }
 612             } else {
 613                 clearToSend = false;
 614             }
 615 #else
 616             clearToSend = false;
 617 #endif
 618         }
 619
 620         if (clearToSend) {
 621             writeUplinkFramePhyID(downlinkPhyID, &emptySmartPortFrame);
 622             clearToSend = false;
 623         }
 624
 625         sendNullFrame = false;
 626     }
 627 #endif
 628
 629     return true;
 630 }
 631
 632
 633 bool fport2RxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig, bool isFBUS)
 634 {
 635     static uint16_t sbusChannelData[SBUS_MAX_CHANNEL];
 636     rxRuntimeConfig->channelData = sbusChannelData;
 637     sbusChannelsInit(rxRuntimeConfig);
 638
 639     rxRuntimeConfig->channelCount = SBUS_MAX_CHANNEL;
 640     rxRuntimeConfig->rcFrameStatusFn = frameStatus;
 641     rxRuntimeConfig->rcProcessFrameFn = processFrame;
 642
 643     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
 644     if (!portConfig) {
 645         return false;
 646     }
 647
 648     uint32_t baudRate = (isFBUS) ? FBUS_BAUDRATE : FPORT2_BAUDRATE;
 649
 650     fportPort = openSerialPort(portConfig->identifier,
 651         FUNCTION_RX_SERIAL,
 652         fportDataReceive,
 653         NULL,
 654         baudRate,
 655         MODE_RXTX,
 656         FPORT2_PORT_OPTIONS | (rxConfig->serialrx_inverted ? 0 : SERIAL_INVERTED) | (tristateWithDefaultOnIsActive(rxConfig->halfDuplex) ? SERIAL_BIDIR : 0)
 657     );
 658
 659     if (fportPort) {
 660 #if defined(USE_TELEMETRY_SMARTPORT)
 661         telemetryEnabled = initSmartPortTelemetryExternal(writeUplinkFrame);
 662 #endif
 663
 664     }
 665
 666     return fportPort != NULL;
 667 }
 668
 669 #endif