src/main/telemetry/msp_shared.c

   1 #include <stdbool.h>
   2 #include <stdint.h>
   3 #include <string.h>
   4
   5 #include "platform.h"
   6
   7 #if defined(USE_MSP_OVER_TELEMETRY)
   8
   9 #include "build/build_config.h"
  10
  11 #include "common/utils.h"
  12
  13 #include "fc/fc_msp.h"
  14
  15 #include "msp/msp.h"
  16
  17 #include "telemetry/crsf.h"
  18 #include "telemetry/msp_shared.h"
  19 #include "telemetry/smartport.h"
  20
  21 #define TELEMETRY_MSP_VERSION    1
  22 #define TELEMETRY_MSP_VER_SHIFT  5
  23 #define TELEMETRY_MSP_VER_MASK   (0x7 << TELEMETRY_MSP_VER_SHIFT)
  24 #define TELEMETRY_MSP_ERROR_FLAG (1 << 5)
  25 #define TELEMETRY_MSP_START_FLAG (1 << 4)
  26 #define TELEMETRY_MSP_SEQ_MASK   0x0F
  27 #define TELEMETRY_MSP_RES_ERROR (-10)
  28
  29 enum {
  30     TELEMETRY_MSP_VER_MISMATCH=0,
  31     TELEMETRY_MSP_CRC_ERROR=1,
  32     TELEMETRY_MSP_ERROR=2
  33 };
  34
  35 STATIC_UNIT_TESTED uint8_t checksum = 0;
  36 STATIC_UNIT_TESTED mspPackage_t mspPackage;
  37 static mspRxBuffer_t mspRxBuffer;
  38 static mspTxBuffer_t mspTxBuffer;
  39 static mspPacket_t mspRxPacket;
  40 static mspPacket_t mspTxPacket;
  41
  42 void initSharedMsp(void)
  43 {
  44     mspPackage.requestBuffer = (uint8_t *)&mspRxBuffer;
  45     mspPackage.requestPacket = &mspRxPacket;
  46     mspPackage.requestPacket->buf.ptr = mspPackage.requestBuffer;
  47     mspPackage.requestPacket->buf.end = mspPackage.requestBuffer;
  48
  49     mspPackage.responseBuffer = (uint8_t *)&mspTxBuffer;
  50     mspPackage.responsePacket = &mspTxPacket;
  51     mspPackage.responsePacket->buf.ptr = mspPackage.responseBuffer;
  52     mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
  53 }
  54
  55 static void processMspPacket(void)
  56 {
  57     mspPackage.responsePacket->cmd = 0;
  58     mspPackage.responsePacket->result = 0;
  59     mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
  60
  61     mspPostProcessFnPtr mspPostProcessFn = NULL;
  62     if (mspFcProcessCommand(mspPackage.requestPacket, mspPackage.responsePacket, &mspPostProcessFn) == MSP_RESULT_ERROR) {
  63         sbufWriteU8(&mspPackage.responsePacket->buf, TELEMETRY_MSP_ERROR);
  64     }
  65     if (mspPostProcessFn) {
  66         mspPostProcessFn(NULL);
  67     }
  68
  69     sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
  70 }
  71
  72 void sendMspErrorResponse(uint8_t error, int16_t cmd)
  73 {
  74     mspPackage.responsePacket->cmd = cmd;
  75     mspPackage.responsePacket->result = 0;
  76     mspPackage.responsePacket->buf.end = mspPackage.responseBuffer;
  77
  78     sbufWriteU8(&mspPackage.responsePacket->buf, error);
  79     mspPackage.responsePacket->result = TELEMETRY_MSP_RES_ERROR;
  80     sbufSwitchToReader(&mspPackage.responsePacket->buf, mspPackage.responseBuffer);
  81 }
  82
  83 bool handleMspFrame(uint8_t *frameStart, int frameLength)
  84 {
  85     static uint8_t mspStarted = 0;
  86     static uint8_t lastSeq = 0;
  87
  88     if (sbufBytesRemaining(&mspPackage.responsePacket->buf) > 0) {
  89         mspStarted = 0;
  90     }
  91
  92     if (mspStarted == 0) {
  93         initSharedMsp();
  94     }
  95
  96     mspPacket_t *packet = mspPackage.requestPacket;
  97     sbuf_t *frameBuf = sbufInit(&mspPackage.requestFrame, frameStart, frameStart + (uint8_t)frameLength);
  98     sbuf_t *rxBuf = &mspPackage.requestPacket->buf;
  99     const uint8_t header = sbufReadU8(frameBuf);
 100     const uint8_t seqNumber = header & TELEMETRY_MSP_SEQ_MASK;
 101     const uint8_t version = (header & TELEMETRY_MSP_VER_MASK) >> TELEMETRY_MSP_VER_SHIFT;
 102
 103     if (version != TELEMETRY_MSP_VERSION) {
 104         sendMspErrorResponse(TELEMETRY_MSP_VER_MISMATCH, 0);
 105         return true;
 106     }
 107
 108     if (header & TELEMETRY_MSP_START_FLAG) {
 109         // first packet in sequence
 110         uint8_t mspPayloadSize = sbufReadU8(frameBuf);
 111
 112         packet->cmd = sbufReadU8(frameBuf);
 113         packet->result = 0;
 114         packet->buf.ptr = mspPackage.requestBuffer;
 115         packet->buf.end = mspPackage.requestBuffer + mspPayloadSize;
 116
 117         checksum = mspPayloadSize ^ packet->cmd;
 118         mspStarted = 1;
 119     } else if (!mspStarted) {
 120         // no start packet yet, throw this one away
 121         return false;
 122     } else if (((lastSeq + 1) & TELEMETRY_MSP_SEQ_MASK) != seqNumber) {
 123         // packet loss detected!
 124         mspStarted = 0;
 125         return false;
 126     }
 127
 128     const uint8_t bufferBytesRemaining = sbufBytesRemaining(rxBuf);
 129     const uint8_t frameBytesRemaining = sbufBytesRemaining(frameBuf);
 130     uint8_t payload[frameBytesRemaining];
 131
 132     if (bufferBytesRemaining >= frameBytesRemaining) {
 133         sbufReadData(frameBuf, payload, frameBytesRemaining);
 134         sbufAdvance(frameBuf, frameBytesRemaining);
 135         sbufWriteData(rxBuf, payload, frameBytesRemaining);
 136         lastSeq = seqNumber;
 137
 138         return false;
 139     } else {
 140         sbufReadData(frameBuf, payload, bufferBytesRemaining);
 141         sbufAdvance(frameBuf, bufferBytesRemaining);
 142         sbufWriteData(rxBuf, payload, bufferBytesRemaining);
 143         sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
 144         while (sbufBytesRemaining(rxBuf)) {
 145             checksum ^= sbufReadU8(rxBuf);
 146         }
 147
 148         if (checksum != *frameBuf->ptr) {
 149             mspStarted = 0;
 150             sendMspErrorResponse(TELEMETRY_MSP_CRC_ERROR, packet->cmd);
 151             return true;
 152         }
 153     }
 154
 155     mspStarted = 0;
 156     sbufSwitchToReader(rxBuf, mspPackage.requestBuffer);
 157     processMspPacket();
 158     return true;
 159 }
 160
 161 bool sendMspReply(uint8_t payloadSize, mspResponseFnPtr responseFn)
 162 {
 163     static uint8_t checksum = 0;
 164     static uint8_t seq = 0;
 165
 166     uint8_t payloadOut[payloadSize];
 167     sbuf_t payload;
 168     sbuf_t *payloadBuf = sbufInit(&payload, payloadOut, payloadOut + payloadSize);
 169     sbuf_t *txBuf = &mspPackage.responsePacket->buf;
 170
 171     // detect first reply packet
 172     if (txBuf->ptr == mspPackage.responseBuffer) {
 173
 174         // header
 175         uint8_t head = TELEMETRY_MSP_START_FLAG | (seq++ & TELEMETRY_MSP_SEQ_MASK);
 176         if (mspPackage.responsePacket->result < 0) {
 177             head |= TELEMETRY_MSP_ERROR_FLAG;
 178         }
 179         sbufWriteU8(payloadBuf, head);
 180
 181         uint8_t size = sbufBytesRemaining(txBuf);
 182         sbufWriteU8(payloadBuf, size);
 183     } else {
 184         // header
 185         sbufWriteU8(payloadBuf, (seq++ & TELEMETRY_MSP_SEQ_MASK));
 186     }
 187
 188     const uint8_t bufferBytesRemaining = sbufBytesRemaining(txBuf);
 189     const uint8_t payloadBytesRemaining = sbufBytesRemaining(payloadBuf);
 190     uint8_t frame[payloadBytesRemaining];
 191
 192     if (bufferBytesRemaining >= payloadBytesRemaining) {
 193
 194         sbufReadData(txBuf, frame, payloadBytesRemaining);
 195         sbufAdvance(txBuf, payloadBytesRemaining);
 196         sbufWriteData(payloadBuf, frame, payloadBytesRemaining);
 197         responseFn(payloadOut);
 198
 199         return true;
 200
 201     } else {
 202
 203         sbufReadData(txBuf, frame, bufferBytesRemaining);
 204         sbufAdvance(txBuf, bufferBytesRemaining);
 205         sbufWriteData(payloadBuf, frame, bufferBytesRemaining);
 206         sbufSwitchToReader(txBuf, mspPackage.responseBuffer);
 207
 208         checksum = sbufBytesRemaining(txBuf) ^ mspPackage.responsePacket->cmd;
 209
 210         while (sbufBytesRemaining(txBuf)) {
 211             checksum ^= sbufReadU8(txBuf);
 212         }
 213         sbufWriteU8(payloadBuf, checksum);
 214
 215         while (sbufBytesRemaining(payloadBuf)>1) {
 216             sbufWriteU8(payloadBuf, 0);
 217         }
 218
 219     }
 220
 221     responseFn(payloadOut);
 222     return false;
 223 }
 224
 225 #endif