diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp index 0a2988c7f7..297d313925 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.cpp @@ -147,7 +147,6 @@ static const char* get_frame_type(uint8_t byte, uint8_t subtype = 0) #define CRSF_INTER_FRAME_TIME_US_250HZ 4000U // At fastest, frames are sent by the transmitter every 4 ms, 250 Hz #define CRSF_INTER_FRAME_TIME_US_150HZ 6667U // At medium, frames are sent by the transmitter every 6.667 ms, 150 Hz #define CRSF_INTER_FRAME_TIME_US_50HZ 20000U // At slowest, frames are sent by the transmitter every 20ms, 50 Hz -#define CSRF_HEADER_LEN 2 // header length #define CSRF_HEADER_TYPE_LEN (CSRF_HEADER_LEN + 1) // header length including type #define CRSF_DIGITAL_CHANNEL_MIN 172 @@ -222,7 +221,7 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) if (_frame_ofs == CSRF_HEADER_TYPE_LEN) { _frame_crc = crc8_dvb_s2(0, _frame.type); // check for garbage frame - if (_frame.length > CRSF_FRAMELEN_MAX) { + if (_frame.length > CRSF_FRAME_PAYLOAD_MAX) { _frame_ofs = 0; } return; @@ -246,8 +245,8 @@ void AP_RCProtocol_CRSF::_process_byte(uint32_t timestamp_us, uint8_t byte) // we consumed the partial frame, reset _frame_ofs = 0; - // bad CRC - if (_frame_crc != _frame.payload[_frame.length - CSRF_HEADER_LEN]) { + // bad CRC (payload start is +1 from frame start, so need to subtract that from frame length to get index) + if (_frame_crc != _frame.payload[_frame.length - 2]) { return; } @@ -434,7 +433,7 @@ void AP_RCProtocol_CRSF::decode_variable_bit_channels(const uint8_t* payload, ui } // calculate the number of channels packed - uint8_t numOfChannels = ((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits; + uint8_t numOfChannels = MIN(uint8_t(((frame_length - 2) * 8 - CRSF_SUBSET_RC_STARTING_CHANNEL_BITS) / channelBits), CRSF_MAX_CHANNELS); // unpack the channel data uint8_t bitsMerged = 0; @@ -443,10 +442,18 @@ void AP_RCProtocol_CRSF::decode_variable_bit_channels(const uint8_t* payload, ui for (uint8_t n = 0; n < numOfChannels; n++) { while (bitsMerged < channelBits) { + // check for corrupt frame + if (readByteIndex >= CRSF_FRAME_PAYLOAD_MAX) { + return; + } uint8_t readByte = payload[readByteIndex++]; readValue |= ((uint32_t) readByte) << bitsMerged; bitsMerged += 8; } + // check for corrupt frame + if (uint8_t(channel_data->starting_channel + n) >= CRSF_MAX_CHANNELS) { + return; + } _channels[channel_data->starting_channel + n] = uint16_t(channelScale * float(uint16_t(readValue & channelMask)) + 988); readValue >>= channelBits; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index 079308ff10..21447d57a0 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -24,6 +24,8 @@ #define CRSF_MAX_CHANNELS 24U // Maximum number of channels from crsf datastream #define CRSF_FRAMELEN_MAX 64U // maximum possible framelength +#define CSRF_HEADER_LEN 2U // header length +#define CRSF_FRAME_PAYLOAD_MAX (CRSF_FRAMELEN_MAX - CSRF_HEADER_LEN) // maximum size of the frame length field in a packet #define CRSF_BAUDRATE 416666 class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { @@ -169,7 +171,7 @@ public: uint8_t device_address; uint8_t length; uint8_t type; - uint8_t payload[CRSF_FRAMELEN_MAX - 3]; // +1 for crc + uint8_t payload[CRSF_FRAME_PAYLOAD_MAX - 1]; // type is already accounted for } PACKED; struct LinkStatisticsFrame { @@ -209,7 +211,7 @@ public: uint8_t starting_channel:5; // which channel number is the first one in the frame uint8_t res_configuration:2; // configuration for the RC data resolution (10 - 13 bits) uint8_t digital_switch_flag:1; // configuration bit for digital channel - uint8_t channels[CRSF_FRAMELEN_MAX - 4]; // +1 for crc + uint8_t channels[CRSF_FRAME_PAYLOAD_MAX - 2]; // payload less byte above // uint16_t channel[]:res; // variable amount of channels (with variable resolution based // on the res_configuration) based on the frame size // uint16_t digital_switch_channel[]:10; // digital switch channel