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)); }