src/main/telemetry/msp_shared.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 #if defined(USE_MSP_OVER_TELEMETRY)
  28
  29 #include "build/build_config.h"
  30
  31 #include "common/utils.h"
  32 #include "common/crc.h"
  33 #include "common/streambuf.h"
  34
  35 #include "msp/msp.h"
  36 #include "msp/msp_protocol.h"
  37 #include "msp/msp_serial.h"
  38
  39 #include "telemetry/crsf.h"
  40 #include "telemetry/msp_shared.h"
  41 #include "telemetry/smartport.h"
  42
  43
  44 /*
  45 ---------------------------------------------------------------
  46 How MSP frames are sent over CRSF:
  47 CRSF frame types: 0x7A for MSP requests, 0x7B for responses.
  48 CRSF extended header frames are used. i.e., Destination and origin addresses added after CRSF type.
  49 CRSF frame structure:
  50 <sync/address><length><type><destination><origin><status><MSP_body><CRC>
  51 Status byte consists of three parts:
  52     bits 0-3 represent the sequence number of the MSP frame,
  53     bit 4 checks if current MSP chunk is the beginning of a new frame (1 if true),
  54     bits 5-6 represent the version number of MSP protocol (MSPv1 or MSPv2)
  55     bit 7 represents Error (1 if there was an error)
  56 MSP_body is unmodified MSP frame without header ($ + M|X + <|>|!) and CRC.
  57 MSP might be MSPv1 or MSPv2 or MSPv1_Jumbo.
  58
  59 MSP_body might be sent in chunks.
  60 First (or only) chunk must always set start bit (#4) of status byte.
  61 Each next chunk must have increased sequence number in status byte.
  62 Size of chunk is recovered from size of CRSF frame.
  63 Although last / only CRSF frame might have size bigger than needed for MSP-body.
  64 Extra bytes must be ignored. So, the real size of MSP-body must be parsed from the MSP-body itself.
  65 CRSF frames might be any size until maximum of 64 bytes for a CRSF frame.
  66 So, maximum chunk size is 57 bytes. Although, MSP-body might be sent in shorter chunks.
  67 Although, first chunk must consist full size any type of the MSP frame.
  68
  69 MSP-CRC is not sent over CRSF due to ther is already CRC of CRSF frame.
  70 So, it must be recalculated of needed for MSP-receiver.
  71
  72 MSP frame must be returned to the origin address of the request
  73
  74 ---------------------------------------------------------------
  75 */
  76 #define TELEMETRY_MSP_VERSION    2
  77 #define TELEMETRY_MSP_RES_ERROR (-10)
  78
  79 #define TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE 5
  80
  81 enum { // constants for status of msp-over-telemetry frame
  82     MSP_STATUS_SEQUENCE_MASK   = 0x0f, // 0b00001111,   // sequence number mask
  83     MSP_STATUS_START_MASK      = 0x10, // 0b00010000,   // bit of starting frame (if 1, the frame is a first/single chunk of msp-frame)
  84     MSP_STATUS_VERSION_MASK    = 0x60, // 0b01100000,   // MSP version mask
  85     MSP_STATUS_ERROR_MASK      = 0x80, // 0b10000000,   // Error bit (1 if error)
  86     MSP_STATUS_VERSION_SHIFT   = 5,    // MSP version shift
  87 };
  88
  89 enum { // error codes (they are not sent anywhere)
  90     TELEMETRY_MSP_VER_MISMATCH,
  91     TELEMETRY_MSP_CRC_ERROR,
  92     TELEMETRY_MSP_ERROR,
  93     TELEMETRY_MSP_REQUEST_IS_TOO_BIG,
  94 };
  95
  96 enum { // minimum length for a frame.
  97     MIN_LENGTH_CHUNK         = 2, // status + at_least_one_byte
  98     MIN_LENGTH_REQUEST_V1    = 3, // status + length + ID
  99     MIN_LENGTH_REQUEST_JUMBO = 5, // status + length=FF + ID + length_lo + length_hi
 100     MIN_LENGTH_REQUEST_V2    = 6, // status + flag + ID_lo + ID_hi + size_lo + size_hi
 101 };
 102
 103 enum { // byte position(index) in msp-over-telemetry request payload
 104     // MSPv1
 105     MSP_INDEX_STATUS        = 0,                           // status byte
 106     MSP_INDEX_SIZE_V1       = MSP_INDEX_STATUS        + 1, // MSPv1 payload size
 107     MSP_INDEX_ID_V1         = MSP_INDEX_SIZE_V1       + 1, // MSPv1 ID/command/function byte
 108     MSP_INDEX_PAYLOAD_V1    = MSP_INDEX_ID_V1         + 1, // MSPv1 Payload start / CRC for zero payload
 109
 110     // MSPv1_Jumbo
 111     MSP_INDEX_SIZE_JUMBO_LO = MSP_INDEX_PAYLOAD_V1,        // MSPv1_Jumbo Lo byte of payload size
 112     MSP_INDEX_SIZE_JUMBO_HI = MSP_INDEX_SIZE_JUMBO_LO + 1, // MSPv1_Jumbo Hi byte of payload size
 113     MSP_INDEX_PAYLOAD_JUMBO = MSP_INDEX_SIZE_JUMBO_HI + 1, // MSPv1_Jumbo first byte of payload itself
 114
 115     // MSPv2
 116     MSP_INDEX_FLAG_V2       = MSP_INDEX_SIZE_V1,           // MSPv2 flags byte
 117     MSP_INDEX_ID_LO         = MSP_INDEX_ID_V1,             // MSPv2 Lo byte of ID/command/function
 118     MSP_INDEX_ID_HI         = MSP_INDEX_ID_LO         + 1, // MSPv2 Hi byte of ID/command/function
 119     MSP_INDEX_SIZE_V2_LO    = MSP_INDEX_ID_HI         + 1, // MSPv2 Lo byte of payload size
 120     MSP_INDEX_SIZE_V2_HI    = MSP_INDEX_SIZE_V2_LO    + 1, // MSPv2 Hi byte of payload size
 121     MSP_INDEX_PAYLOAD_V2    = MSP_INDEX_SIZE_V2_HI    + 1, // MSPv2 first byte of payload itself
 122 };
 123
 124 STATIC_UNIT_TESTED uint8_t requestBuffer[MSP_TLM_INBUF_SIZE];
 125 STATIC_UNIT_TESTED uint8_t responseBuffer[MSP_TLM_OUTBUF_SIZE];
 126 STATIC_UNIT_TESTED mspPacket_t requestPacket;
 127 STATIC_UNIT_TESTED mspPacket_t responsePacket;
 128 static uint8_t lastRequestVersion; // MSP version of last request. Temporary solution. It's better to keep it in requestPacket.
 129
 130 static mspDescriptor_t mspSharedDescriptor = -1;
 131
 132 void initSharedMsp(void)
 133 {
 134     responsePacket.buf.ptr = responseBuffer;
 135     responsePacket.buf.end = ARRAYEND(responseBuffer);
 136
 137     mspSharedDescriptor = mspDescriptorAlloc();
 138 }
 139
 140 mspDescriptor_t getMspTelemetryDescriptor(void)
 141 {
 142     return mspSharedDescriptor;
 143 }
 144
 145 static void processMspPacket(void)
 146 {
 147     responsePacket.cmd = 0;
 148     responsePacket.result = 0;
 149     responsePacket.buf.ptr = responseBuffer;
 150     responsePacket.buf.end = ARRAYEND(responseBuffer);
 151
 152     mspPostProcessFnPtr mspPostProcessFn = NULL;
 153     if (mspFcProcessCommand(mspSharedDescriptor, &requestPacket, &responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
 154         sbufWriteU8(&responsePacket.buf, TELEMETRY_MSP_ERROR);
 155     }
 156     if (mspPostProcessFn) {
 157         mspPostProcessFn(NULL);
 158     }
 159
 160     sbufSwitchToReader(&responsePacket.buf, responseBuffer);
 161 }
 162
 163 void sendMspErrorResponse(uint8_t error, int16_t cmd)
 164 {
 165     responsePacket.cmd = cmd;
 166     responsePacket.result = 0;
 167     responsePacket.buf.ptr = responseBuffer;
 168
 169     sbufWriteU8(&responsePacket.buf, error);
 170     responsePacket.result = TELEMETRY_MSP_RES_ERROR;
 171     sbufSwitchToReader(&responsePacket.buf, responseBuffer);
 172 }
 173
 174 // despite its name, the function actually handles telemetry frame payload with MSP in it
 175 // it reads the MSP into requestPacket stucture and handles it after receiving all the chunks.
 176 bool handleMspFrame(uint8_t *const payload, uint8_t const payloadLength, uint8_t *const skipsBeforeResponse)
 177 {
 178     if (payloadLength < MIN_LENGTH_CHUNK) {
 179         return false;   // prevent analyzing garbage data
 180     }
 181
 182     static uint8_t mspStarted = 0;
 183     static uint8_t lastSeq = 0;
 184
 185     sbuf_t sbufInput;
 186
 187     const uint8_t status = payload[MSP_INDEX_STATUS];
 188     const uint8_t seqNumber = status & MSP_STATUS_SEQUENCE_MASK;
 189     lastRequestVersion = (status & MSP_STATUS_VERSION_MASK) >> MSP_STATUS_VERSION_SHIFT;
 190
 191     if (lastRequestVersion > TELEMETRY_MSP_VERSION) {
 192         sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
 193         return true;
 194     }
 195
 196     if (status & MSP_STATUS_START_MASK) { // first packet in sequence
 197         uint16_t mspPayloadSize;
 198         if (lastRequestVersion == 1) { // MSPv1
 199             if (payloadLength < MIN_LENGTH_REQUEST_V1) {
 200                 return false;   // prevent analyzing garbage data
 201             }
 202
 203             mspPayloadSize = payload[MSP_INDEX_SIZE_V1];
 204             requestPacket.cmd = payload[MSP_INDEX_ID_V1];
 205             if (mspPayloadSize == 0xff) { // jumbo frame
 206                 if (payloadLength < MIN_LENGTH_REQUEST_JUMBO) {
 207                     return false;   // prevent analyzing garbage data
 208                 }
 209                 mspPayloadSize = *(uint16_t*)&payload[MSP_INDEX_SIZE_JUMBO_LO];
 210                 sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_JUMBO, payload + payloadLength);
 211             } else {
 212                 sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_V1, payload + payloadLength);
 213             }
 214         } else { // MSPv2
 215             if (payloadLength < MIN_LENGTH_REQUEST_V2) {
 216                 return false;   // prevent analyzing garbage data
 217             }
 218             requestPacket.flags = payload[MSP_INDEX_FLAG_V2];
 219             requestPacket.cmd = *(uint16_t*)&payload[MSP_INDEX_ID_LO];
 220             mspPayloadSize = *(uint16_t*)&payload[MSP_INDEX_SIZE_V2_LO];
 221             sbufInit(&sbufInput, payload + MSP_INDEX_PAYLOAD_V2, payload + payloadLength);
 222         }
 223         if (mspPayloadSize <= sizeof(requestBuffer)) { // prevent buffer overrun
 224             requestPacket.result = 0;
 225             requestPacket.buf.ptr = requestBuffer;
 226             requestPacket.buf.end = requestBuffer + mspPayloadSize;
 227             mspStarted = 1;
 228         } else { // this MSP packet is too big to fit in the buffer.
 229             sendMspErrorResponse(TELEMETRY_MSP_REQUEST_IS_TOO_BIG, requestPacket.cmd);
 230             return true;
 231         }
 232     } else { // second onward chunk
 233         if (!mspStarted) { // no start packet yet, throw this one away
 234             return false;
 235         } else {
 236             if (((lastSeq + 1) & MSP_STATUS_SEQUENCE_MASK) != seqNumber) {
 237                 // packet loss detected!
 238                 mspStarted = 0;
 239                 return false;
 240             }
 241         }
 242         sbufInit(&sbufInput, payload + 1, payload + payloadLength);
 243     }
 244
 245     lastSeq = seqNumber;
 246
 247     const int payloadExpecting = sbufBytesRemaining(&requestPacket.buf);
 248     const int payloadIncoming = sbufBytesRemaining(&sbufInput);
 249
 250     if (payloadExpecting > payloadIncoming) {
 251         sbufWriteData(&requestPacket.buf, sbufInput.ptr, payloadIncoming);
 252         sbufAdvance(&sbufInput, payloadIncoming);
 253         return false;
 254     } else { // this is the last/only chunk
 255         if (payloadExpecting) {
 256             sbufWriteData(&requestPacket.buf, sbufInput.ptr, payloadExpecting);
 257             sbufAdvance(&sbufInput, payloadExpecting);
 258         }
 259     }
 260
 261     // Skip a few telemetry requests if command is MSP_EEPROM_WRITE
 262     if (requestPacket.cmd == MSP_EEPROM_WRITE && skipsBeforeResponse) {
 263         *skipsBeforeResponse = TELEMETRY_REQUEST_SKIPS_AFTER_EEPROMWRITE;
 264     }
 265
 266     mspStarted = 0;
 267     sbufSwitchToReader(&requestPacket.buf, requestBuffer);
 268     processMspPacket();
 269     return true;
 270 }
 271
 272 bool sendMspReply(const uint8_t payloadSizeMax, mspResponseFnPtr responseFn)
 273 {
 274     static uint8_t seq = 0;
 275
 276     uint8_t payloadArray[payloadSizeMax];
 277     sbuf_t payloadBufStruct;
 278     sbuf_t *payloadBuf = sbufInit(&payloadBufStruct, payloadArray, payloadArray + payloadSizeMax);
 279
 280     // detect first reply packet
 281     if (responsePacket.buf.ptr == responseBuffer) {
 282         // this is the first frame of the response packet. Add proper header and size.
 283         // header
 284         uint8_t status = MSP_STATUS_START_MASK | (seq++ & MSP_STATUS_SEQUENCE_MASK) | (lastRequestVersion << MSP_STATUS_VERSION_SHIFT);
 285         if (responsePacket.result < 0) {
 286             status |= MSP_STATUS_ERROR_MASK;
 287         }
 288         sbufWriteU8(payloadBuf, status);
 289
 290         const int size = sbufBytesRemaining(&responsePacket.buf);  // size might be bigger than 0xff
 291         if (lastRequestVersion == 1) { // MSPv1
 292             if (size >= 0xff) {
 293                 // Sending Jumbo-frame
 294                 sbufWriteU8(payloadBuf, 0xff);
 295                 sbufWriteU8(payloadBuf, responsePacket.cmd);
 296                 sbufWriteU16(payloadBuf, (uint16_t)size);
 297             } else {
 298                 sbufWriteU8(payloadBuf, size);
 299                 sbufWriteU8(payloadBuf, responsePacket.cmd);
 300             }
 301         } else { // MSPv2
 302             sbufWriteU8 (payloadBuf, responsePacket.flags);  // MSPv2 flags
 303             sbufWriteU16(payloadBuf, responsePacket.cmd);    // command is 16 bit in MSPv2
 304             sbufWriteU16(payloadBuf, (uint16_t)size);        // size is 16 bit in MSPv2
 305         }
 306     } else {
 307         sbufWriteU8(payloadBuf, (seq++ & MSP_STATUS_SEQUENCE_MASK) | (lastRequestVersion << MSP_STATUS_VERSION_SHIFT)); // header without 'start' flag
 308     }
 309
 310     const int inputRemainder = sbufBytesRemaining(&responsePacket.buf);// size might be bigger than 0xff
 311     const int chunkRemainder = sbufBytesRemaining(payloadBuf); // free space remainder for current chunk
 312
 313     if (inputRemainder >= chunkRemainder) {
 314         // partial send
 315         sbufWriteData(payloadBuf, responsePacket.buf.ptr, chunkRemainder);
 316         sbufAdvance(&responsePacket.buf, chunkRemainder);
 317         responseFn(payloadArray, payloadSizeMax);
 318         return true;
 319     }
 320     // last/only chunk
 321     sbufWriteData(payloadBuf, responsePacket.buf.ptr, inputRemainder);
 322     sbufAdvance(&responsePacket.buf, inputRemainder);
 323     sbufSwitchToReader(&responsePacket.buf, responseBuffer);// for CRC calculation
 324
 325     responseFn(payloadArray, payloadBuf->ptr - payloadArray);
 326     return false;
 327 }
 328
 329 #endif