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 int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, const uint8_t * data, int dataLen, const uint8_t * crc, int crcLen)
{
// We are allowed to send out the response if
// a) TX buffer is completely empty (we are talking to well-behaving party that follows request-response scheduling;
// this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
// b) Response fits into TX buffer
const int totalFrameLength = hdrLen + dataLen + crcLen;
if (!isSerialTransmitBufferEmpty(msp->port) && ((int)serialTxBytesFree(msp->port) < totalFrameLength))
return 0;
// Transmit frame
serialBeginWrite(msp->port);
serialWriteBuf(msp->port, hdr, hdrLen);
serialWriteBuf(msp->port, data, dataLen);
serialWriteBuf(msp->port, crc, crcLen);
serialEndWrite(msp->port);
return totalFrameLength;
}
static void ltm_finalise(sbuf_t *dst)
{
sbufWriteU8(dst, ltm_crc);
sbufSwitchToReader(dst, ltmPayload);
serialWriteBuf(ltmPort, sbufPtr(dst), sbufBytesRemaining(dst));
}
void serialWriteBufShim(void *instance, const uint8_t *data, int count)
{
serialWriteBuf((serialPort_t *)instance, data, count);
}