void mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
{
serialBeginWrite(msp->port);
int len = sbufBytesRemaining(&packet->buf);
uint8_t hdr[] = {'$', 'M', packet->result < 0 ? '!' : (msp->mode == MSP_MODE_SERVER ? '>' : '<'), len, packet->cmd};
uint8_t csum = 0; // initial checksum value
serialWriteBuf(msp->port, hdr, sizeof(hdr));
csum = mspSerialChecksumBuf(csum, hdr + 3, 2); // checksum starts from len field
if(len > 0) {
serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
csum = mspSerialChecksumBuf(csum, sbufPtr(&packet->buf), len);
}
serialWrite(msp->port, csum);
serialEndWrite(msp->port);
}
static void srxlFinalize(sbuf_t *dst)
{
crc16_ccitt_sbuf_append(dst, &srxlFrame[3]); // start at byte 3, since CRC does not include device address and packet length
sbufSwitchToReader(dst, srxlFrame);
// write the telemetry frame to the receiver.
srxlRxWriteTelemetryData(sbufPtr(dst), sbufBytesRemaining(dst));
}
void crc16_ccitt_sbuf_append(sbuf_t *dst, uint8_t *start)
{
uint16_t crc = 0;
const uint8_t * const end = sbufPtr(dst);
for (const uint8_t *ptr = start; ptr < end; ++ptr) {
crc = crc16_ccitt(crc, *ptr);
}
sbufWriteU16(dst, crc);
}
static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e mspVersion)
{
static const uint8_t mspMagic[MSP_VERSION_COUNT] = MSP_VERSION_MAGIC_INITIALIZER;
const int dataLen = sbufBytesRemaining(&packet->buf);
uint8_t hdrBuf[16] = { '$', mspMagic[mspVersion], packet->result == MSP_RESULT_ERROR ? '!' : '>'};
uint8_t crcBuf[2];
uint8_t checksum;
int hdrLen = 3;
int crcLen = 0;
#define V1_CHECKSUM_STARTPOS 3
if (mspVersion == MSP_V1) {
mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
hdrLen += sizeof(mspHeaderV1_t);
hdrV1->cmd = packet->cmd;
// Add JUMBO-frame header if necessary
if (dataLen >= JUMBO_FRAME_SIZE_LIMIT) {
mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen];
hdrLen += sizeof(mspHeaderJUMBO_t);
hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
hdrJUMBO->size = dataLen;
}
else {
hdrV1->size = dataLen;
}
// Pre-calculate CRC
checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
crcBuf[crcLen++] = checksum;
}
else if (mspVersion == MSP_V2_OVER_V1) {
mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
hdrLen += sizeof(mspHeaderV1_t);
mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
hdrLen += sizeof(mspHeaderV2_t);
const int v1PayloadSize = sizeof(mspHeaderV2_t) + dataLen + 1; // MSPv2 header + data payload + MSPv2 checksum
hdrV1->cmd = MSP_V2_FRAME_ID;
// Add JUMBO-frame header if necessary
if (v1PayloadSize >= JUMBO_FRAME_SIZE_LIMIT) {
mspHeaderJUMBO_t * hdrJUMBO = (mspHeaderJUMBO_t *)&hdrBuf[hdrLen];
hdrLen += sizeof(mspHeaderJUMBO_t);
hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
hdrJUMBO->size = v1PayloadSize;
}
else {
hdrV1->size = v1PayloadSize;
}
// Fill V2 header
hdrV2->flags = packet->flags;
hdrV2->cmd = packet->cmd;
hdrV2->size = dataLen;
// V2 CRC: only V2 header + data payload
checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
crcBuf[crcLen++] = checksum;
// V1 CRC: All headers + data payload + V2 CRC byte
checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
checksum = mspSerialChecksumBuf(checksum, crcBuf, crcLen);
crcBuf[crcLen++] = checksum;
}
else if (mspVersion == MSP_V2_NATIVE) {
mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
hdrLen += sizeof(mspHeaderV2_t);
hdrV2->flags = packet->flags;
hdrV2->cmd = packet->cmd;
hdrV2->size = dataLen;
checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
crcBuf[crcLen++] = checksum;
}
else {
// Shouldn't get here
return 0;
}
// Send the frame
return mspSerialSendFrame(msp, hdrBuf, hdrLen, sbufPtr(&packet->buf), dataLen, crcBuf, crcLen);
}
static void ltm_finalise(sbuf_t *dst)
{
sbufWriteU8(dst, ltm_crc);
sbufSwitchToReader(dst, ltmPayload);
serialWriteBuf(ltmPort, sbufPtr(dst), sbufBytesRemaining(dst));
}
