mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
Added extra error checking - making sure the packet size is valid and that packet sizes match.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2040 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
pip
pip
parent
ed03a45ee5
commit
b6c0344a0f
@ -56,11 +56,18 @@ const quint8 UAVTalk::crc_table[256] = {
|
||||
UAVTalk::UAVTalk(QIODevice* iodev, UAVObjectManager* objMngr)
|
||||
{
|
||||
io = iodev;
|
||||
|
||||
this->objMngr = objMngr;
|
||||
|
||||
rxState = STATE_SYNC;
|
||||
rxPacketLength = 0;
|
||||
|
||||
mutex = new QMutex(QMutex::Recursive);
|
||||
|
||||
respObj = NULL;
|
||||
|
||||
memset(&stats, 0, sizeof(ComStats));
|
||||
|
||||
connect(io, SIGNAL(readyRead()), this, SLOT(processInputStream()));
|
||||
}
|
||||
|
||||
@ -187,86 +194,119 @@ bool UAVTalk::objectTransaction(UAVObject* obj, quint8 type, bool allInstances)
|
||||
bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
{
|
||||
// Update stats
|
||||
++stats.rxBytes;
|
||||
stats.rxBytes++;
|
||||
|
||||
rxPacketLength++; // update packet byte count
|
||||
|
||||
// Receive state machine
|
||||
switch (rxState) {
|
||||
case STATE_SYNC:
|
||||
if (rxbyte == SYNC_VAL) {
|
||||
rxCS = updateCRC(0, &rxbyte, 1);
|
||||
rxState = STATE_TYPE;
|
||||
}
|
||||
break;
|
||||
case STATE_TYPE:
|
||||
if ((rxbyte & TYPE_MASK) == TYPE_VER )
|
||||
switch (rxState)
|
||||
{
|
||||
rxCS = updateCRC(rxCS, &rxbyte, 1);
|
||||
case STATE_SYNC:
|
||||
|
||||
if (rxbyte != SYNC_VAL)
|
||||
break;
|
||||
|
||||
// Initialize and update CRC
|
||||
rxCS = updateCRC(0, rxbyte);
|
||||
|
||||
rxPacketLength = 1;
|
||||
|
||||
rxState = STATE_TYPE;
|
||||
break;
|
||||
|
||||
case STATE_TYPE:
|
||||
|
||||
// Update CRC
|
||||
rxCS = updateCRC(rxCS, rxbyte);
|
||||
|
||||
if ((rxbyte & TYPE_MASK) != TYPE_VER)
|
||||
{
|
||||
rxState = STATE_SYNC;
|
||||
break;
|
||||
}
|
||||
|
||||
rxType = rxbyte;
|
||||
|
||||
packetSize = 0;
|
||||
|
||||
rxState = STATE_SIZE;
|
||||
rxCount = 0;
|
||||
} else {
|
||||
rxState = STATE_SYNC;
|
||||
}
|
||||
break;
|
||||
|
||||
case STATE_SIZE:
|
||||
if(rxCount++ == 0) {
|
||||
rxCS = updateCRC(rxCS, &rxbyte, 1);
|
||||
|
||||
// Update CRC
|
||||
rxCS = updateCRC(rxCS, rxbyte);
|
||||
|
||||
if (rxCount == 0)
|
||||
{
|
||||
packetSize += rxbyte;
|
||||
rxCount++;
|
||||
break;
|
||||
}
|
||||
else {
|
||||
rxCS = updateCRC(rxCS, &rxbyte, 1);
|
||||
rxCount++;
|
||||
packetSize += rxbyte << 8;
|
||||
|
||||
packetSize += (quint32)rxbyte << 8;
|
||||
|
||||
if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH)
|
||||
{ // incorrect packet size
|
||||
rxState = STATE_SYNC;
|
||||
break;
|
||||
}
|
||||
|
||||
rxCount = 0;
|
||||
rxState = STATE_OBJID;
|
||||
}
|
||||
break;
|
||||
|
||||
case STATE_OBJID:
|
||||
|
||||
// Update CRC
|
||||
rxCS = updateCRC(rxCS, rxbyte);
|
||||
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 4)
|
||||
{
|
||||
if (rxCount < 4)
|
||||
break;
|
||||
|
||||
// Search for object, if not found reset state machine
|
||||
rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
|
||||
{
|
||||
UAVObject *rxObj = objMngr->getObject(rxObjId);
|
||||
if (rxObj == NULL)
|
||||
{
|
||||
stats.rxErrors++;
|
||||
rxState = STATE_SYNC;
|
||||
++stats.rxErrors;
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Update checksum
|
||||
rxCS = updateCRC(rxCS, rxTmpBuffer, 4);
|
||||
|
||||
// Determine data length
|
||||
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK)
|
||||
{
|
||||
rxLength = 0;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxLength = rxObj->getNumBytes();
|
||||
}
|
||||
|
||||
// Check length and determine next state
|
||||
if (rxLength >= MAX_PAYLOAD_LENGTH)
|
||||
{
|
||||
stats.rxErrors++;
|
||||
rxState = STATE_SYNC;
|
||||
++stats.rxErrors;
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
// Check the lengths match
|
||||
if ((rxPacketLength + rxLength) != packetSize)
|
||||
{ // packet error - mismatched packet size
|
||||
stats.rxErrors++;
|
||||
rxState = STATE_SYNC;
|
||||
break;
|
||||
}
|
||||
|
||||
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
|
||||
if (rxObj->isSingleInstance())
|
||||
{
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (rxLength > 0)
|
||||
{
|
||||
rxState = STATE_DATA;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_CS;
|
||||
}
|
||||
rxInstId = 0;
|
||||
rxCount = 0;
|
||||
}
|
||||
@ -276,55 +316,74 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
|
||||
rxCount = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE_INSTID:
|
||||
|
||||
// Update CRC
|
||||
rxCS = updateCRC(rxCS, rxbyte);
|
||||
|
||||
rxTmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 2)
|
||||
{
|
||||
if (rxCount < 2)
|
||||
break;
|
||||
|
||||
rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);
|
||||
rxCS = updateCRC(rxCS, rxTmpBuffer, 2);
|
||||
|
||||
rxCount = 0;
|
||||
|
||||
// If there is a payload get it, otherwise receive checksum
|
||||
if (rxLength > 0)
|
||||
{
|
||||
rxState = STATE_DATA;
|
||||
}
|
||||
else
|
||||
{
|
||||
rxState = STATE_CS;
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE_DATA:
|
||||
|
||||
// Update CRC
|
||||
rxCS = updateCRC(rxCS, rxbyte);
|
||||
|
||||
rxBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == rxLength)
|
||||
{
|
||||
rxCS = updateCRC(rxCS, rxBuffer, rxLength);
|
||||
if (rxCount < rxLength)
|
||||
break;
|
||||
|
||||
rxState = STATE_CS;
|
||||
rxCount = 0;
|
||||
}
|
||||
break;
|
||||
|
||||
case STATE_CS:
|
||||
|
||||
// The CRC byte
|
||||
rxCSPacket = rxbyte;
|
||||
if (rxCS == rxCSPacket)
|
||||
{
|
||||
|
||||
if (rxCS != rxCSPacket)
|
||||
{ // packet error - faulty CRC
|
||||
stats.rxErrors++;
|
||||
rxState = STATE_SYNC;
|
||||
break;
|
||||
}
|
||||
|
||||
if (rxPacketLength != packetSize + 1)
|
||||
{ // packet error - mismatched packet size
|
||||
stats.rxErrors++;
|
||||
rxState = STATE_SYNC;
|
||||
break;
|
||||
}
|
||||
|
||||
mutex->lock();
|
||||
receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
|
||||
stats.rxObjectBytes += rxLength;
|
||||
++stats.rxObjects;
|
||||
stats.rxObjects++;
|
||||
mutex->unlock();
|
||||
}
|
||||
else
|
||||
{
|
||||
++stats.rxErrors;
|
||||
}
|
||||
|
||||
rxState = STATE_SYNC;
|
||||
break;
|
||||
|
||||
default:
|
||||
rxState = STATE_SYNC;
|
||||
++stats.rxErrors;
|
||||
stats.rxErrors++;
|
||||
}
|
||||
|
||||
// Done
|
||||
@ -654,15 +713,14 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance
|
||||
* \param length Number of bytes in the \a data buffer.
|
||||
* \return The updated crc value.
|
||||
*/
|
||||
quint8 UAVTalk::updateCRC(quint8 crc, const quint8 data)
|
||||
{
|
||||
return crc_table[crc ^ data];
|
||||
}
|
||||
quint8 UAVTalk::updateCRC(quint8 crc, const quint8* data, qint32 length)
|
||||
{
|
||||
quint32 tbl_idx;
|
||||
|
||||
while (length--) {
|
||||
tbl_idx = ((crc >> 0) ^ *data) & 0xff;
|
||||
crc = (crc_table[tbl_idx] ^ (crc << 8)) & 0xff;
|
||||
data++;
|
||||
}
|
||||
while (length--)
|
||||
crc = crc_table[crc ^ *data++];
|
||||
return crc;
|
||||
}
|
||||
|
||||
|
||||
@ -72,10 +72,16 @@ private:
|
||||
static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
|
||||
static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
|
||||
static const int TYPE_ACK = (TYPE_VER | 0x03);
|
||||
static const int HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID (2, not used in single objects)
|
||||
|
||||
static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4)
|
||||
static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects)
|
||||
|
||||
static const int CHECKSUM_LENGTH = 1;
|
||||
|
||||
static const int MAX_PAYLOAD_LENGTH = 256;
|
||||
static const int MAX_PACKET_LENGTH = (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH);
|
||||
|
||||
static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
|
||||
|
||||
static const quint16 ALL_INSTANCES = 0xFFFF;
|
||||
static const int TX_BUFFER_SIZE = 2*1024;
|
||||
static const quint8 crc_table[256];
|
||||
@ -97,6 +103,7 @@ private:
|
||||
quint32 rxObjId;
|
||||
quint16 rxInstId;
|
||||
quint16 rxLength;
|
||||
quint16 rxPacketLength;
|
||||
|
||||
quint8 rxCSPacket, rxCS;
|
||||
qint32 rxCount;
|
||||
@ -112,6 +119,7 @@ private:
|
||||
void updateAck(UAVObject* obj);
|
||||
bool transmitObject(UAVObject* obj, quint8 type, bool allInstances);
|
||||
bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances);
|
||||
quint8 updateCRC(quint8 crc, const quint8 data);
|
||||
quint8 updateCRC(quint8 crc, const quint8* data, qint32 length);
|
||||
|
||||
};
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user