mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-26 01:35:41 +03:00
Merge pull request #9780 from etracer65/mspv2_buffer_overrun_check
This commit is contained in:
parent
ea310c149b
commit
2af0f2b1e6
1 changed files with 20 additions and 24 deletions
|
@ -103,8 +103,7 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
|
||||||
case MSP_IDLE: // Waiting for '$' character
|
case MSP_IDLE: // Waiting for '$' character
|
||||||
if (c == '$') {
|
if (c == '$') {
|
||||||
mspPort->c_state = MSP_HEADER_START;
|
mspPort->c_state = MSP_HEADER_START;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
@ -172,12 +171,10 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
|
||||||
if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
|
if (hdr->size >= sizeof(mspHeaderV2_t) + 1) {
|
||||||
mspPort->mspVersion = MSP_V2_OVER_V1;
|
mspPort->mspVersion = MSP_V2_OVER_V1;
|
||||||
mspPort->c_state = MSP_HEADER_V2_OVER_V1;
|
mspPort->c_state = MSP_HEADER_V2_OVER_V1;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
mspPort->c_state = MSP_IDLE;
|
mspPort->c_state = MSP_IDLE;
|
||||||
}
|
}
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
mspPort->dataSize = hdr->size;
|
mspPort->dataSize = hdr->size;
|
||||||
mspPort->cmdMSP = hdr->cmd;
|
mspPort->cmdMSP = hdr->cmd;
|
||||||
mspPort->cmdFlags = 0;
|
mspPort->cmdFlags = 0;
|
||||||
|
@ -209,11 +206,15 @@ static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
|
||||||
mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
|
mspPort->checksum2 = crc8_dvb_s2(mspPort->checksum2, c);
|
||||||
if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
|
if (mspPort->offset == (sizeof(mspHeaderV2_t) + sizeof(mspHeaderV1_t))) {
|
||||||
mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
|
mspHeaderV2_t * hdrv2 = (mspHeaderV2_t *)&mspPort->inBuf[sizeof(mspHeaderV1_t)];
|
||||||
mspPort->dataSize = hdrv2->size;
|
if (hdrv2->size > MSP_PORT_INBUF_SIZE) {
|
||||||
mspPort->cmdMSP = hdrv2->cmd;
|
mspPort->c_state = MSP_IDLE;
|
||||||
mspPort->cmdFlags = hdrv2->flags;
|
} else {
|
||||||
mspPort->offset = 0; // re-use buffer
|
mspPort->dataSize = hdrv2->size;
|
||||||
mspPort->c_state = mspPort->dataSize > 0 ? MSP_PAYLOAD_V2_OVER_V1 : MSP_CHECKSUM_V2_OVER_V1;
|
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;
|
break;
|
||||||
|
|
||||||
|
@ -286,8 +287,9 @@ static int mspSerialSendFrame(mspPort_t *msp, const uint8_t * hdr, int hdrLen, c
|
||||||
// this allows us to transmit jumbo frames bigger than TX buffer (serialWriteBuf will block, but for jumbo frames we don't care)
|
// 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
|
// b) Response fits into TX buffer
|
||||||
const int totalFrameLength = hdrLen + dataLen + crcLen;
|
const int totalFrameLength = hdrLen + dataLen + crcLen;
|
||||||
if (!isSerialTransmitBufferEmpty(msp->port) && ((int)serialTxBytesFree(msp->port) < totalFrameLength))
|
if (!isSerialTransmitBufferEmpty(msp->port) && ((int)serialTxBytesFree(msp->port) < totalFrameLength)) {
|
||||||
return 0;
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
// Transmit frame
|
// Transmit frame
|
||||||
serialBeginWrite(msp->port);
|
serialBeginWrite(msp->port);
|
||||||
|
@ -322,8 +324,7 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp
|
||||||
|
|
||||||
hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
|
hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
|
||||||
hdrJUMBO->size = dataLen;
|
hdrJUMBO->size = dataLen;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
hdrV1->size = dataLen;
|
hdrV1->size = dataLen;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -331,8 +332,7 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp
|
||||||
checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
|
checksum = mspSerialChecksumBuf(0, hdrBuf + V1_CHECKSUM_STARTPOS, hdrLen - V1_CHECKSUM_STARTPOS);
|
||||||
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
|
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
|
||||||
crcBuf[crcLen++] = checksum;
|
crcBuf[crcLen++] = checksum;
|
||||||
}
|
} else if (mspVersion == MSP_V2_OVER_V1) {
|
||||||
else if (mspVersion == MSP_V2_OVER_V1) {
|
|
||||||
mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
|
mspHeaderV1_t * hdrV1 = (mspHeaderV1_t *)&hdrBuf[hdrLen];
|
||||||
|
|
||||||
hdrLen += sizeof(mspHeaderV1_t);
|
hdrLen += sizeof(mspHeaderV1_t);
|
||||||
|
@ -350,8 +350,7 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp
|
||||||
|
|
||||||
hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
|
hdrV1->size = JUMBO_FRAME_SIZE_LIMIT;
|
||||||
hdrJUMBO->size = v1PayloadSize;
|
hdrJUMBO->size = v1PayloadSize;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
hdrV1->size = v1PayloadSize;
|
hdrV1->size = v1PayloadSize;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -370,8 +369,7 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp
|
||||||
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
|
checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), dataLen);
|
||||||
checksum = mspSerialChecksumBuf(checksum, crcBuf, crcLen);
|
checksum = mspSerialChecksumBuf(checksum, crcBuf, crcLen);
|
||||||
crcBuf[crcLen++] = checksum;
|
crcBuf[crcLen++] = checksum;
|
||||||
}
|
} else if (mspVersion == MSP_V2_NATIVE) {
|
||||||
else if (mspVersion == MSP_V2_NATIVE) {
|
|
||||||
mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
|
mspHeaderV2_t * hdrV2 = (mspHeaderV2_t *)&hdrBuf[hdrLen];
|
||||||
hdrLen += sizeof(mspHeaderV2_t);
|
hdrLen += sizeof(mspHeaderV2_t);
|
||||||
|
|
||||||
|
@ -382,8 +380,7 @@ static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet, mspVersion_e msp
|
||||||
checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
|
checksum = crc8_dvb_s2_update(0, (uint8_t *)hdrV2, sizeof(mspHeaderV2_t));
|
||||||
checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
|
checksum = crc8_dvb_s2_update(checksum, sbufPtr(&packet->buf), dataLen);
|
||||||
crcBuf[crcLen++] = checksum;
|
crcBuf[crcLen++] = checksum;
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
// Shouldn't get here
|
// Shouldn't get here
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
@ -528,8 +525,7 @@ void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessComm
|
||||||
waitForSerialPortToFinishTransmitting(mspPort->port);
|
waitForSerialPortToFinishTransmitting(mspPort->port);
|
||||||
mspPostProcessFn(mspPort->port);
|
mspPostProcessFn(mspPort->port);
|
||||||
}
|
}
|
||||||
}
|
} else {
|
||||||
else {
|
|
||||||
mspProcessPendingRequest(mspPort);
|
mspProcessPendingRequest(mspPort);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue