bool ResponseCollector::matchExpected(DataBuffer& data) { //make a save point so we can revert if need be ReadBufferSavePoint savepoint(&data); size_t startBytesRemaining = 0; //get a lock on the response mutex mutex_lock_guard lock(m_responseMutex); //need to loop over all of the bytes to check for responses while(data.moreToRead()) { //log the bytes remaining in the data before we attempt to match startBytesRemaining = data.bytesRemaining(); //look through all the expected responses for(auto itr = m_expectedResponses.begin(); itr < m_expectedResponses.end(); itr++) { //don't try to match bytes that were in the buffer before the command was sent if(data.readPosition() < itr->minBytePosition) { continue; } //if we found a match if(itr->pattern->match(data)) { //match functions move the data pointer on success (even if not fully matched) //need to make sure we don't roll back to our previous savepoint. savepoint.commit(); if(itr->pattern->fullyMatched()) { //unregister the response m_expectedResponses.erase(itr); return true; } } //if any match function moved the data pointer, we don't want to move it back here if(startBytesRemaining != data.bytesRemaining()) { savepoint.commit(); } } //if we can move to the next byte and the byte position hasn't been moved elsewhere (internally by a match function). if(data.moreToRead() && startBytesRemaining == data.bytesRemaining()) { //move to the next byte and check for the response again data.read_uint8(); } } return false; }
bool WirelessParser::findPacketInBytes(DataBuffer& data, WirelessTypes::Frequency freq) { //create a read save point for the DataBuffer ReadBufferSavePoint savePoint(&data); std::size_t lastReadPosition; //while there are enough bytes remaining to make an ASPP response packet while(data.bytesRemaining() > WirelessPacket::ASPP_MIN_RESPONSE_PACKET_SIZE) { lastReadPosition = data.readPosition(); //move to the next byte data.read_uint8(); WirelessPacket packet; //if we found a packet within the bytes if(parseAsPacket(data, packet, freq) == parsePacketResult_completePacket) { //commit the data that was read savePoint.commit(); //process the packet processPacket(packet, lastReadPosition); return true; } } //we didn't find any packet in the bytes buffer return false; }
bool BaseStation_SetBeacon::Response::match(DataBuffer& data) { const uint16 TOTAL_SUCCESS_BYTES = 2; //create a save point with the data ReadBufferSavePoint savePoint(&data); //if there aren't enough bytes in the buffer to match the response if(data.bytesRemaining() < TOTAL_SUCCESS_BYTES) { return false; } //if it doesn't match the command Id if(data.read_uint16() != COMMAND_ID) { return false; } //if we made it this far, we successfully matched everything //commit the current read position savePoint.commit(); //set the result to success m_success = true; //we have fully matched the response m_fullyMatched = true; //notify that the response was matched m_matchCondition.notify(); return true; }
bool InertialParser::findPacketInBytes(DataBuffer& data) { //create a read save point for the DataBuffer ReadBufferSavePoint savePoint(&data); //while there are enough bytes remaining to make a MIP packet while(data.bytesRemaining() > InertialPacketInfo::MIP_MIN_PACKET_SIZE) { //move to the next byte data.read_uint8(); InertialPacket packet; //if we found a packet within the bytes if(parseAsPacket(data, packet) == inertialParserResult_completePacket) { //commit the data that was read savePoint.commit(); //process the packet processPacket(packet); return true; } } //we didn't find any packet in the bytes buffer return false; }
bool ReadSingleSensor::Response::match(DataBuffer& data) { const uint16 TOTAL_BYTES = 5; //if there aren't enough bytes in the buffer to match the response if(data.bytesRemaining() < TOTAL_BYTES) { //not a good response m_success = false; return false; } //create a save point with the data ReadBufferSavePoint savePoint(&data); //verify the command id if(data.read_uint8() != 0x03) { //not a good response m_success = false; return false; } uint16 sensorVal = data.read_uint16(); ChecksumBuilder checksum; checksum.append_uint16(sensorVal); //value of the requested channel //verify the checksum (only a checksum on the actual data value) if(checksum.simpleChecksum() != data.read_uint16()) { //not a good response m_success = false; return false; } //if we made it this far, the bytes match the expected response m_success = true; m_sensorValue = sensorVal; //commit the current read position savePoint.commit(); //we have fully matched the response m_fullyMatched = true; //notify that the response was matched m_matchCondition.notify(); return true; }
bool ShortPing::Response::match(DataBuffer& data) { const uint16 TOTAL_BYTES = 1; //if there aren't enough bytes in the buffer to match the response if(data.bytesRemaining() < TOTAL_BYTES) { //not a good response m_success = false; return false; } //create a save point with the data ReadBufferSavePoint savePoint(&data); uint8 resultByte = data.read_uint8(); //single byte 0x21 is a fail response if(resultByte == 0x21) { //legitimate fail response m_success = false; } //single byte 0x02 is a success response else if(resultByte == WirelessProtocol::cmdId_shortPing) { //success response m_success = true; } else { //non-matching response m_success = false; return false; } //if we made it this far, the bytes match the expected response (success or fail) //commit the current read position savePoint.commit(); //we have fully matched the response m_fullyMatched = true; //notify that the response was matched m_matchCondition.notify(); return true; }
WirelessParser::ParsePacketResult WirelessParser::parseAsPacket(DataBuffer& data, WirelessPacket& packet, WirelessTypes::Frequency freq) { std::size_t totalBytesAvailable = data.bytesRemaining(); if(totalBytesAvailable == 0) { //Not Enough Data to tell if valid packet return parsePacketResult_notEnoughData; } //choose the correct ASPP version parser switch(data.peekByte()) { case WirelessPacket::ASPP_V1_START_OF_PACKET_BYTE: return parseAsPacket_ASPP_v1(data, packet, freq); case WirelessPacket::ASPP_V2_START_OF_PACKET_BYTE: return parseAsPacket_ASPP_v2(data, packet, freq); default: return parsePacketResult_invalidPacket; } }
WirelessParser::ParsePacketResult WirelessParser::parseAsPacket_ASPP_v2(DataBuffer& data, WirelessPacket& packet, WirelessTypes::Frequency freq) { //Assume we are at the start of the packet, read the packet header //byte 1 - Start Of Packet //byte 2 - Delivery Stop Flag //byte 3 - App Data Type //byte 4 - 7 - Node Address (uint32) //byte 8 - 9 - Payload Length //byte 10 to N-4 - Payload //byte N-3 - Node RSSI //byte N-2 - Base RSSI //byte N-1 - Fletcher Checksum (MSB) //byte N - Fletcher Checksum (LSB) //create a save point for the DataBuffer ReadBufferSavePoint savePoint(&data); std::size_t totalBytesAvailable = data.bytesRemaining(); //we need at least 13 bytes for any ASPP v2 packet (if empty payload) if(totalBytesAvailable < 13) { //Not Enough Data to tell if valid packet return parsePacketResult_notEnoughData; } //read byte 1 uint8 startOfPacket = data.read_uint8(); //Start Of Packet //verify that the first byte is the Start Of Packet if(startOfPacket != WirelessPacket::ASPP_V2_START_OF_PACKET_BYTE) { //Invalid Packet return parsePacketResult_invalidPacket; } //read byte 2 uint8 deliveryStopFlag = data.read_uint8(); //Delivery Stop Flag //read byte 3 uint8 appDataType = data.read_uint8(); //App Data Type //read bytes 4 - 7 uint32 nodeAddress = data.read_uint32(); //Node Address //read bytes 8 and 9 uint16 payloadLength = data.read_uint16(); //Payload Length //determine the full packet length size_t packetLength = payloadLength + WirelessPacket::ASPP_V2_NUM_BYTES_BEFORE_PAYLOAD + WirelessPacket::ASPP_V2_NUM_BYTES_AFTER_PAYLOAD; //the DataBuffer must be large enough to hold the rest of the packet if(totalBytesAvailable < packetLength) { //Not Enough Data to tell if valid packet return parsePacketResult_notEnoughData; } //create the Bytes vector to hold the payload bytes Bytes payload; payload.reserve(payloadLength); //loop through the payload for(uint16 payloadItr = 0; payloadItr < payloadLength; payloadItr++) { //store the payload bytes payload.push_back(data.read_uint8()); //Payload Bytes } //read the node RSSI uint8 nodeRSSI = data.read_uint8(); //Node RSSI //read the base station rssi uint8 baseRSSI = data.read_uint8(); //Base RSSI //get the checksum sent in the packet uint16 checksum = data.read_uint16(); //Checksum //build the checksum to calculate from all the bytes ChecksumBuilder calcChecksum; calcChecksum.append_uint8(startOfPacket); calcChecksum.append_uint8(deliveryStopFlag); calcChecksum.append_uint8(appDataType); calcChecksum.append_uint32(nodeAddress); calcChecksum.append_uint16(payloadLength); calcChecksum.appendBytes(payload); calcChecksum.append_uint8(nodeRSSI); calcChecksum.append_uint8(baseRSSI); //verify that the returned checksum is the same as the one we calculated if(checksum != calcChecksum.fletcherChecksum()) { //Bad Checksum return parsePacketResult_badChecksum; } DeliveryStopFlags flags = DeliveryStopFlags::fromByte(deliveryStopFlag); //add all the info about the packet to the WirelessPacket reference passed in packet.deliveryStopFlags(flags); packet.type(static_cast<WirelessPacket::PacketType>(appDataType)); packet.nodeAddress(nodeAddress); packet.payload(payload); packet.nodeRSSI(static_cast<int16>(nodeRSSI) - 205); packet.baseRSSI(static_cast<int16>(baseRSSI) - 205); packet.frequency(freq); //Correct the packet type if it is incorrect WirelessPacketUtils::correctPacketType(packet); //make sure the packet is valid based on its specific type if(!WirelessPacketUtils::packetIntegrityCheck(packet)) { //not a valid packet, failed integrity check return parsePacketResult_invalidPacket; } //check if the packet is a duplicate if(isDuplicate(packet)) { //even though it is a duplicate, we still have a complete packet so commit the bytes to skip over them savePoint.commit(); //duplicate packet return parsePacketResult_duplicate; } //we have a complete packet, commit the bytes that we just read (move the read pointer) savePoint.commit(); return parsePacketResult_completePacket; }
void WirelessParser::parse(DataBuffer& data, WirelessTypes::Frequency freq) { ParsePacketResult parseResult; //holds the result of verifying whether it was a valid ASPP packet or not WirelessPacket packet; size_t bytesRemaining; //holds how many bytes we have remaining, helps determine if the buffer has been moved by an external function //make a save point so we can revert if need be ReadBufferSavePoint savepoint(&data); std::size_t lastReadPosition; //while there is more data to be read in the DataBuffer while(data.moreToRead()) { lastReadPosition = data.readPosition(); //read the next byte (doesn't move data's read position) uint8 currentByte = data.peekByte(); //skipByte is set to false when we don't want to skip to the next byte after we are done looking at the current byte bool moveToNextByte = true; //notEnoughData is true when the bytes could be a valid packet, but there isn't enough bytes to be sure bool notEnoughData = false; //if this is any ASPP Start of Packet byte if(currentByte == WirelessPacket::ASPP_V1_START_OF_PACKET_BYTE || currentByte == WirelessPacket::ASPP_V2_START_OF_PACKET_BYTE) { //check if the packet is a valid ASPP packet, starting at this byte parseResult = parseAsPacket(data, packet, freq); //check the result of the parseAsPacket command switch(parseResult) { //good packet, process it and then look for the next case parsePacketResult_completePacket: processPacket(packet, lastReadPosition); savepoint.commit(); continue; //packet has been processed, move to the next byte after the packet case parsePacketResult_duplicate: savepoint.commit(); continue; //packet is a duplicate, but byte position has been moved. Move to the next byte after the packet //somethings incorrect in the packet, move passed the AA and start looking for the next packet case parsePacketResult_invalidPacket: case parsePacketResult_badChecksum: savepoint.commit(); break; //ran out of data, return and wait for more case parsePacketResult_notEnoughData: moveToNextByte = false; notEnoughData = true; break; default: assert(false); //unhandled verifyResult, need to add a case for it } } //data is not a packet at this point bytesRemaining = data.bytesRemaining(); //check if the bytes we currently have match an expected response // This isn't perfect (could possibly use part of a partial ASPP packet as a cmd response), // but unfortunately its the best we can do with single byte responses being part of our protocol if(findMatchingResponse(data)) { //the bytes have already moved, don't move to the next byte in the next iteration savepoint.commit(); moveToNextByte = false; } else { //failed to match //if we didn't have enough data for a full packet, and it didn't match any expected responses if(notEnoughData) { //look for packets after the current byte. // Even though this looks like it could be the start of an ASPP packet, // if we find any full ASPP packets inside of the these bytes, we need // to pick them up and move on. if(!findPacketInBytes(data, freq)) { //if the read position in the bytes has been moved external to this function if(data.bytesRemaining() != bytesRemaining) { //read position has moved somewhere else, so bytes have been committed. Commit in our local savepoint as well. savepoint.commit(); } //we didn't find a packet within this, so return from this function as we need to wait for more data return; } else { savepoint.commit(); } } } //if we need to move to the next byte if (moveToNextByte) { //if the read position in the bytes has been moved external to this function if(data.bytesRemaining() != bytesRemaining) { //read position has moved somewhere else, so bytes have been committed. Commit in our local savepoint as well. savepoint.commit(); } else { //move to the next byte data.read_uint8(); } } } }
InertialParseResult InertialParser::parseAsPacket(DataBuffer& data, InertialPacket& packet) { //Assume that we are at the start of a packet //byte 1 - Start of Packet 1 (0x75) //byte 2 - Start of Packet 2 (0x65) //byte 3 - Descriptor Set //byte 4 - Payload Length //byte 5 to N-2 - Payload //byte N-1 - Checksum (MSB) //byte N - Checksum (LSB) //create a save point for the DataBuffer ReadBufferSavePoint savePoint(&data); std::size_t totalBytesAvailable = data.bytesRemaining(); //make sure we have enough bytes to even be a MIP packet if(totalBytesAvailable < InertialPacketInfo::MIP_MIN_PACKET_SIZE) { //Not Enough Data to tell if valid packet return inertialParserResult_notEnoughData; } //read the first 2 bytes uint16 startOfPacket = data.read_uint16(); //Start of Packet //verify that the Start of Packet value is correct if(startOfPacket != InertialPacketInfo::INERTIAL_PACKET_START_OF_PACKET) { //Invalid Packet return inertialParserResult_invalidPacket; } //read byte 3 uint8 descriptorSet = data.read_uint8(); //Descriptor Set //read byte 4 uint8 payloadLen = data.read_uint8(); //Payload Length //determine the full packet length uint32 packetLength = payloadLen + InertialPacketInfo::MIP_NUM_BYTES_BEFORE_PAYLOAD + InertialPacketInfo::MIP_NUM_BYTES_AFTER_PAYLOAD; //the DataBuffer must be large enough to hold the rest of the packet if(totalBytesAvailable < packetLength) { //Not Enough Data to tell if valid packet return inertialParserResult_notEnoughData; } //create the Bytes vector to hold the payload bytes Bytes payload; payload.reserve(payloadLen); //loop through the payload for(uint8 payloadItr = 0; payloadItr < payloadLen; payloadItr++) { //store the payload bytes payload.push_back(data.read_uint8()); //Payload Bytes } //get the checksum sent in the packet uint16 checksum = data.read_uint16(); //Checksum //build the checksum to calculate from all the bytes ChecksumBuilder calcChecksum; calcChecksum.append_uint16(startOfPacket); calcChecksum.append_uint8(descriptorSet); calcChecksum.append_uint8(payloadLen); calcChecksum.appendBytes(payload); if(checksum != calcChecksum.fletcherChecksum()) { //Bad Checksum return inertialParserResult_badChecksum; } //add all the info about the packet to the InertialPacket reference passed in packet.descriptorSet(descriptorSet); packet.payload(payload); //we have a complete packet, commit the bytes that we just read (move the read pointer) savePoint.commit(); return inertialParserResult_completePacket; }