uint8_t crsf_frame_CRC(const crsf_frame_t &frame)
{
// CRC includes type and payload
uint8_t crc = crc8_dvb_s2(0, frame.type);
for (int i = 0; i < frame.header.length - 2; ++i) {
crc = crc8_dvb_s2(crc, frame.payload[i]);
}
return crc;
}
void crc8_dvb_s2_sbuf_append(sbuf_t *dst, uint8_t *start)
{
uint8_t crc = 0;
const uint8_t * const end = dst->ptr;
for (const uint8_t *ptr = start; ptr < end; ++ptr) {
crc = crc8_dvb_s2(crc, *ptr);
}
sbufWriteU8(dst, crc);
}
uint8_t crc8_dvb_s2_update(uint8_t crc, const void *data, uint32_t length)
{
const uint8_t *p = (const uint8_t *)data;
const uint8_t *pend = p + length;
for (; p != pend; p++) {
crc = crc8_dvb_s2(crc, *p);
}
return crc;
}
static uint8_t crc8_dvb_s2_buf(uint8_t *buf, int len)
{
uint8_t crc = 0;
for (int i = 0; i < len; ++i) {
crc = crc8_dvb_s2(crc, buf[i]);
}
return crc;
}
static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
switch (mspPort->c_state) {
default:
case MSP_IDLE: // Waiting for '$' character
if (c == '$') {
mspPort->c_state = MSP_HEADER_START;
}
else {
return false;
}
break;
case MSP_HEADER_START: // Waiting for 'M' (MSPv1 / MSPv2_over_v1) or 'X' (MSPv2 native)
mspPort->offset = 0;
mspPort->checksum1 = 0;
mspPort->checksum2 = 0;
switch (c) {
case 'M':
mspPort->c_state = MSP_HEADER_M;
mspPort->mspVersion = MSP_V1;
break;
case 'X':
mspPort->c_state = MSP_HEADER_X;
mspPort->mspVersion = MSP_V2_NATIVE;
break;
default:
mspPort->c_state = MSP_IDLE;
break;
}
break;
case MSP_HEADER_M: // Waiting for '<' / '>'
mspPort->c_state = MSP_HEADER_V1;
switch (c) {
case '<':
mspPort->packetType = MSP_PACKET_COMMAND;
break;
case '>':
mspPort->packetType = MSP_PACKET_REPLY;
break;
default:
mspPort->c_state = MSP_IDLE;
break;
}
break;
case MSP_HEADER_X:
mspPort->c_state = MSP_HEADER_V2_NATIVE;
switch (c) {
case '<':
mspPort->packetType = MSP_PACKET_COMMAND;
break;
case '>':
mspPort->packetType = MSP_PACKET_REPLY;
break;
default:
mspPort->c_state = MSP_IDLE;
break;
}
break;
case MSP_HEADER_V1: // Now receive v1 header (size/cmd), this is already checksummable
mspPort->inBuf[mspPort->offset++] = c;
mspPort->checksum1 ^= c;
if (mspPort->offset == sizeof(mspHeaderV1_t)) {
mspHeaderV1_t * hdr = (mspHeaderV1_t *)&mspPort->inBuf[0];
// Check incoming buffer size limit
if (hdr->size > MSP_PORT_INBUF_SIZE) {
mspPort->c_state = MSP_IDLE;
}
else if (hdr->cmd == MSP_V2_FRAME_ID) {
// MSPv1 payload must be big enough to hold V2 header + extra checksum
if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
mspPort->mspVersion = MSP_V2_OVER_V1;
mspPort->c_state = MSP_HEADER_V2_OVER_V1;
}
else {
mspPort->c_state = MSP_IDLE;
}
}
else {
mspPort->dataSize = hdr->size;
mspPort->cmdMSP = hdr->cmd;
mspPort->cmdFlags = 0;
mspPort->offset = 0; // re-use buffer
mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V1 : MSP_CHECKSUM_V1; // If no payload - jump to checksum byte
}
}
break;
case MSP_PAYLOAD_V1:
mspPort->inBuf[mspPort->offset++] = c;
mspPort->checksum1 ^= c;
if (mspPort->offset == mspPort->dataSize) {
mspPort->c_state = MSP_CHECKSUM_V1;
}
break;
case MSP_CHECKSUM_V1:
if (mspPort->checksum1 == c) {
mspPort->c_state = MSP_COMMAND_RECEIVED;
} else {
mspPort->c_state = MSP_IDLE;
}
break;
case MSP_HEADER_V2_OVER_V1: // V2 header is part of V1 payload - we need to calculate both checksums now
mspPort->inBuf[mspPort->offset++] = c;
mspPort->checksum1 ^= c;
mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
mspPort->dataSize = hdrv2->size;
mspPort->cmdMSP = hdrv2->cmd;
mspPort->cmdFlags = hdrv2->flags;
mspPort->offset = 0; // re-use buffer
mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
}
break;
case MSP_PAYLOAD_V2_OVER_V1:
mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
mspPort->checksum1 ^= c;
mspPort->inBuf[mspPort->offset++] = c;
if (mspPort->offset == mspPort->dataSize) {
mspPort->c_state = MSP_CHECKSUM_V2_OVER_V1;
}
break;
case MSP_CHECKSUM_V2_OVER_V1:
mspPort->checksum1 ^= c;
if (mspPort->checksum2 == c) {
mspPort->c_state = MSP_CHECKSUM_V1; // Checksum 2 correct - verify v1 checksum
} else {
mspPort->c_state = MSP_IDLE;
}
break;
case MSP_HEADER_V2_NATIVE:
mspPort->inBuf[mspPort->offset++] = c;
mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
if (mspPort->offset == sizeof(mspHeaderV2_t)) {
mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[0];
mspPort->dataSize = hdrv2->size;
mspPort->cmdMSP = hdrv2->cmd;
mspPort->cmdFlags = hdrv2->flags;
mspPort->offset = 0; // re-use buffer
mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_NATIVE : MSP_CHECKSUM_V2_NATIVE;
}
break;
case MSP_PAYLOAD_V2_NATIVE:
mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
mspPort->inBuf[mspPort->offset++] = c;
if (mspPort->offset == mspPort->dataSize) {
mspPort->c_state = MSP_CHECKSUM_V2_NATIVE;
}
break;
case MSP_CHECKSUM_V2_NATIVE:
if (mspPort->checksum2 == c) {
mspPort->c_state = MSP_COMMAND_RECEIVED;
} else {
mspPort->c_state = MSP_IDLE;
}
break;
}
return true;
}
static void crsfSerialize8(sbuf_t *dst, uint8_t v)
{
sbufWriteU8(dst, v);
crsfCrc = crc8_dvb_s2(crsfCrc, v);
}
