From 9f43e9e39eda1f768c8ac01a3632d523ff6626fd Mon Sep 17 00:00:00 2001 From: Paul Hollinsky Date: Thu, 13 Sep 2018 19:39:19 -0400 Subject: [PATCH] Separate MessageDecoder from Packetizer and optimize This will, in the future, allow overriding of MessageDecoder per device as necessary. --- CMakeLists.txt | 2 + communication/communication.cpp | 7 +- communication/include/communication.h | 6 + communication/include/messagedecoder.h | 484 ++++++++++---------- communication/include/packetizer.h | 38 ++ communication/message/include/message.h | 1 + communication/messagedecoder.cpp | 145 +----- communication/multichannelcommunication.cpp | 20 +- communication/packetizer.cpp | 111 +++++ 9 files changed, 440 insertions(+), 374 deletions(-) create mode 100644 communication/include/packetizer.h create mode 100644 communication/packetizer.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ad344ab..9036da5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -30,7 +30,9 @@ else() endif() set(COMMON_SRC + communication/message/neomessage.cpp communication/messagedecoder.cpp + communication/packetizer.cpp communication/multichannelcommunication.cpp communication/communication.cpp communication/icommunication.cpp diff --git a/communication/communication.cpp b/communication/communication.cpp index 96c2e71..5e0392c 100644 --- a/communication/communication.cpp +++ b/communication/communication.cpp @@ -5,6 +5,7 @@ #include #include #include "communication/include/messagedecoder.h" +#include "communication/include/packetizer.h" using namespace icsneo; @@ -86,13 +87,15 @@ bool Communication::removeMessageCallback(int id) { void Communication::readTask() { std::vector readBytes; + Packetizer packetizer; MessageDecoder decoder; while(!closing) { readBytes.clear(); if(impl->readWait(readBytes)) { - if(decoder.input(readBytes)) { - for(auto& msg : decoder.output()) { + if(packetizer.input(readBytes)) { + for(auto& packet : packetizer.output()) { + auto msg = decoder.decodePacket(packet); for(auto& cb : messageCallbacks) { // We might have closed while reading or processing if(!closing) { cb.second.callIfMatch(msg); diff --git a/communication/include/communication.h b/communication/include/communication.h index bb5643d..92b5fad 100644 --- a/communication/include/communication.h +++ b/communication/include/communication.h @@ -40,6 +40,12 @@ public: void setAlign16Bit(bool enable) { align16bit = enable; } + class Packet { + public: + Network network; + std::vector data; + }; + protected: std::shared_ptr impl; static int messageCallbackIDCounter; diff --git a/communication/include/messagedecoder.h b/communication/include/messagedecoder.h index 53f596f..3d7e693 100644 --- a/communication/include/messagedecoder.h +++ b/communication/include/messagedecoder.h @@ -1,6 +1,7 @@ #ifndef __MESSAGEDECODER_H_ #define __MESSAGEDECODER_H_ +#include "communication/include/communication.h" #include "communication/message/include/message.h" #include "communication/message/include/canmessage.h" #include "communication/include/network.h" @@ -12,245 +13,258 @@ namespace icsneo { class MessageDecoder { public: - bool input(const std::vector& bytes); - std::vector> output(); + std::shared_ptr decodePacket(const std::shared_ptr& message); private: - enum class ReadState { - SearchForHeader, - ParseHeader, - ParseLongStylePacketHeader, - GetData - }; - ReadState state = ReadState::SearchForHeader; - - int currentIndex = 0; - int messageLength = 0; - int headerSize = 0; - bool checksum = false; - bool gotGoodMessages = false; // Tracks whether we've ever gotten a good message - Message message; - std::deque bytes; - - void processMessage(const Message& message); - - std::vector> processedMessages; - typedef uint16_t icscm_bitfield; - struct CoreMiniMsg { - CANMessage toCANMessage(Network netid); - union { - uint16_t CxTRB0SID16; - struct - { - icscm_bitfield IDE : 1; - icscm_bitfield SRR : 1; - icscm_bitfield SID : 11; - icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS - } CxTRB0SID; - struct - { - icscm_bitfield : 13; - icscm_bitfield EDL : 1; - icscm_bitfield BRS : 1; - icscm_bitfield ESI : 1; - } CxTRB0FD; - struct - { - icscm_bitfield ErrRxOnlyBreak : 1; - icscm_bitfield ErrRxOnlyBreakSync : 1; - icscm_bitfield ID : 11; - icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS - } CxLIN3; - struct - { - uint8_t D8; - uint8_t options : 4; - uint8_t TXMSG : 1; - uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS - } C1xJ1850; - struct - { - uint8_t D8; - uint8_t options : 4; - uint8_t TXMSG : 1; - uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS - } C1xISO; - struct - { - uint8_t D8; - uint8_t options : 4; - uint8_t TXMSG : 1; - uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS - } C1xJ1708; - struct - { - icscm_bitfield FCS_AVAIL : 1; - icscm_bitfield RUNT_FRAME : 1; - icscm_bitfield DISABLE_PADDING : 1; - icscm_bitfield PREEMPTION_ENABLED : 1; - icscm_bitfield MPACKET_TYPE : 4; - icscm_bitfield MPACKET_FRAG_CNT : 2; - icscm_bitfield : 6; - } C1xETH; - struct - { - uint16_t ID : 11; - uint16_t STARTUP : 1; - uint16_t SYNC : 1; - uint16_t NULL_FRAME : 1; - uint16_t PAYLOAD_PREAMBLE : 1; - uint16_t RESERVED_0 : 1; - } C1xFlex; - struct - { - uint8_t daqType; - uint8_t ethDaqRes1; - } C1xETHDAQ; - }; - union { - uint16_t CxTRB0EID16; - struct - { - icscm_bitfield EID : 12; - icscm_bitfield TXMSG : 1; - icscm_bitfield TXAborted : 1; - icscm_bitfield TXLostArb : 1; - icscm_bitfield TXError : 1; - } CxTRB0EID; - struct - { - uint8_t LINByte9; - uint8_t ErrTxRxMismatch : 1; - uint8_t TxChkSumEnhanced : 1; - uint8_t TXMaster : 1; - uint8_t TXSlave : 1; - uint8_t ErrRxBreakNot0 : 1; - uint8_t ErrRxBreakTooShort : 1; - uint8_t ErrRxSyncNot55 : 1; - uint8_t ErrRxDataGreater8 : 1; - } CxLIN; - struct - { - uint8_t D9; - uint8_t D10; - } C2xJ1850; - struct - { - uint8_t D9; - uint8_t D10; - } C2xISO; - struct - { - uint8_t D9; - uint8_t D10; - } C2xJ1708; - struct - { - uint16_t txlen : 12; - uint16_t TXMSG : 1; - uint16_t : 3; - } C2xETH; - struct - { - uint16_t HDR_CRC_10 : 1; - uint16_t PAYLOAD_LEN : 7; - uint16_t RESERVED_1 : 4; - uint16_t TXMSG : 1; - uint16_t RESERVED_2 : 3; - } C2xFlex; - }; - union { - // For use by CAN - uint16_t CxTRB0DLC16; - struct - { - icscm_bitfield DLC : 4; - icscm_bitfield RB0 : 1; - icscm_bitfield IVRIF : 1; - icscm_bitfield HVEnable : 1;// must be cleared before passing into CAN driver - icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS - icscm_bitfield RB1 : 1; - icscm_bitfield RTR : 1; - icscm_bitfield EID : 6; - } CxTRB0DLC; - struct - { - icscm_bitfield len : 4; - icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS - icscm_bitfield UpdateSlaveOnce : 1; - icscm_bitfield HasUpdatedSlaveOnce : 1; - icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS - icscm_bitfield BusRecovered : 1; - icscm_bitfield SyncFerr : 1;//!< We got framing error in our sync byte. - icscm_bitfield MidFerr : 1;//!< We got framing error in our message id. - icscm_bitfield SlaveByteFerr : 1;//!< We got framing error in one of our slave bytes. - icscm_bitfield TxAborted : 1;//!< This transmit was aborted. - icscm_bitfield breakOnly : 1; - icscm_bitfield : 2; - } CxLIN2; - // For use by JVPW - struct - { - icscm_bitfield len : 4; - icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS - icscm_bitfield just_tx_timestamp : 1; - icscm_bitfield first_seg : 1; - icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit - icscm_bitfield D11 : 8; - } C3xJ1850; - // For use by the ISO/KEYWORD - struct - { - icscm_bitfield len : 4; - icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS - icscm_bitfield FRM : 1; - icscm_bitfield INIT : 1; - icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit - icscm_bitfield D11 : 8; - } C3xISO; - struct - { - icscm_bitfield len : 4; - icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS - icscm_bitfield FRM : 1; - icscm_bitfield : 1; - icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit - icscm_bitfield pri : 8; - } C3xJ1708; - struct - { - uint16_t rsvd; - } C3xETH; - struct - { - uint16_t CYCLE : 6; - uint16_t HDR_CRC_9_0 : 10; - } C3xFlex; - }; - unsigned char CxTRB0Dall[8]; - union { - uint16_t CxTRB0STAT; - uint16_t J1850_TX_ID; - }; - union { - struct - { - uint32_t uiTimeStamp10uS; - union { - uint32_t uiTimeStamp10uSMSB; - struct - { - unsigned : 28; - unsigned res_0s : 3;// must be 0!!! - unsigned bIsExtended : 1;// always 1 for CoreMiniMsgExtended. - }; - }; - }; - int64_t uiTimeStampLarge; - uint8_t uiTimeStampBytes[8]; - }; + struct HardwareCANPacket { + struct { + icscm_bitfield IDE : 1; + icscm_bitfield SRR : 1; + icscm_bitfield SID : 11; + icscm_bitfield EDL : 1; + icscm_bitfield BRS : 1; + icscm_bitfield ESI : 1; + } header; + struct { + icscm_bitfield EID : 12; + icscm_bitfield TXMSG : 1; + icscm_bitfield TXAborted : 1; + icscm_bitfield TXLostArb : 1; + icscm_bitfield TXError : 1; + } eid; + struct { + icscm_bitfield DLC : 4; + icscm_bitfield RB0 : 1; + icscm_bitfield IVRIF : 1; + icscm_bitfield HVEnable : 1;// must be cleared before passing into CAN driver + icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS + icscm_bitfield RB1 : 1; + icscm_bitfield RTR : 1; + icscm_bitfield EID : 6; + } dlc; + unsigned char data[8]; + struct { + uint64_t TS : 60; + uint64_t : 3; + uint64_t IsExtended : 1; + } timestamp; }; + + // struct CoreMiniMsg { + // CANMessage toCANMessage(Network netid); + // union { + // uint16_t CxTRB0SID16; + // struct + // { + // icscm_bitfield IDE : 1; + // icscm_bitfield SRR : 1; + // icscm_bitfield SID : 11; + // icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS + // } CxTRB0SID; + // struct + // { + // icscm_bitfield : 13; + // icscm_bitfield EDL : 1; + // icscm_bitfield BRS : 1; + // icscm_bitfield ESI : 1; + // } CxTRB0FD; + // struct + // { + // icscm_bitfield ErrRxOnlyBreak : 1; + // icscm_bitfield ErrRxOnlyBreakSync : 1; + // icscm_bitfield ID : 11; + // icscm_bitfield NETWORKINDEX : 3;//DO NOT CLOBBER THIS + // } CxLIN3; + // struct + // { + // uint8_t D8; + // uint8_t options : 4; + // uint8_t TXMSG : 1; + // uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS + // } C1xJ1850; + // struct + // { + // uint8_t D8; + // uint8_t options : 4; + // uint8_t TXMSG : 1; + // uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS + // } C1xISO; + // struct + // { + // uint8_t D8; + // uint8_t options : 4; + // uint8_t TXMSG : 1; + // uint8_t NETWORKINDEX : 3;//DO NOT CLOBBER THIS + // } C1xJ1708; + // struct + // { + // icscm_bitfield FCS_AVAIL : 1; + // icscm_bitfield RUNT_FRAME : 1; + // icscm_bitfield DISABLE_PADDING : 1; + // icscm_bitfield PREEMPTION_ENABLED : 1; + // icscm_bitfield MPACKET_TYPE : 4; + // icscm_bitfield MPACKET_FRAG_CNT : 2; + // icscm_bitfield : 6; + // } C1xETH; + // struct + // { + // uint16_t ID : 11; + // uint16_t STARTUP : 1; + // uint16_t SYNC : 1; + // uint16_t NULL_FRAME : 1; + // uint16_t PAYLOAD_PREAMBLE : 1; + // uint16_t RESERVED_0 : 1; + // } C1xFlex; + // struct + // { + // uint8_t daqType; + // uint8_t ethDaqRes1; + // } C1xETHDAQ; + // }; + // union { + // uint16_t CxTRB0EID16; + // struct + // { + // icscm_bitfield EID : 12; + // icscm_bitfield TXMSG : 1; + // icscm_bitfield TXAborted : 1; + // icscm_bitfield TXLostArb : 1; + // icscm_bitfield TXError : 1; + // } CxTRB0EID; + // struct + // { + // uint8_t LINByte9; + // uint8_t ErrTxRxMismatch : 1; + // uint8_t TxChkSumEnhanced : 1; + // uint8_t TXMaster : 1; + // uint8_t TXSlave : 1; + // uint8_t ErrRxBreakNot0 : 1; + // uint8_t ErrRxBreakTooShort : 1; + // uint8_t ErrRxSyncNot55 : 1; + // uint8_t ErrRxDataGreater8 : 1; + // } CxLIN; + // struct + // { + // uint8_t D9; + // uint8_t D10; + // } C2xJ1850; + // struct + // { + // uint8_t D9; + // uint8_t D10; + // } C2xISO; + // struct + // { + // uint8_t D9; + // uint8_t D10; + // } C2xJ1708; + // struct + // { + // uint16_t txlen : 12; + // uint16_t TXMSG : 1; + // uint16_t : 3; + // } C2xETH; + // struct + // { + // uint16_t HDR_CRC_10 : 1; + // uint16_t PAYLOAD_LEN : 7; + // uint16_t RESERVED_1 : 4; + // uint16_t TXMSG : 1; + // uint16_t RESERVED_2 : 3; + // } C2xFlex; + // }; + // union { + // // For use by CAN + // uint16_t CxTRB0DLC16; + // struct + // { + // icscm_bitfield DLC : 4; + // icscm_bitfield RB0 : 1; + // icscm_bitfield IVRIF : 1; + // icscm_bitfield HVEnable : 1;// must be cleared before passing into CAN driver + // icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS + // icscm_bitfield RB1 : 1; + // icscm_bitfield RTR : 1; + // icscm_bitfield EID : 6; + // } CxTRB0DLC; + // struct + // { + // icscm_bitfield len : 4; + // icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS + // icscm_bitfield UpdateSlaveOnce : 1; + // icscm_bitfield HasUpdatedSlaveOnce : 1; + // icscm_bitfield ExtendedNetworkIndexBit : 1;//DO NOT CLOBBER THIS + // icscm_bitfield BusRecovered : 1; + // icscm_bitfield SyncFerr : 1;//!< We got framing error in our sync byte. + // icscm_bitfield MidFerr : 1;//!< We got framing error in our message id. + // icscm_bitfield SlaveByteFerr : 1;//!< We got framing error in one of our slave bytes. + // icscm_bitfield TxAborted : 1;//!< This transmit was aborted. + // icscm_bitfield breakOnly : 1; + // icscm_bitfield : 2; + // } CxLIN2; + // // For use by JVPW + // struct + // { + // icscm_bitfield len : 4; + // icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS + // icscm_bitfield just_tx_timestamp : 1; + // icscm_bitfield first_seg : 1; + // icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit + // icscm_bitfield D11 : 8; + // } C3xJ1850; + // // For use by the ISO/KEYWORD + // struct + // { + // icscm_bitfield len : 4; + // icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS + // icscm_bitfield FRM : 1; + // icscm_bitfield INIT : 1; + // icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit + // icscm_bitfield D11 : 8; + // } C3xISO; + // struct + // { + // icscm_bitfield len : 4; + // icscm_bitfield ExtendedNetworkIndexBit2 : 1;//DO NOT CLOBBER THIS + // icscm_bitfield FRM : 1; + // icscm_bitfield : 1; + // icscm_bitfield ExtendedNetworkIndexBit : 1;// do not clobber ExtendedNetworkIndexBit + // icscm_bitfield pri : 8; + // } C3xJ1708; + // struct + // { + // uint16_t rsvd; + // } C3xETH; + // struct + // { + // uint16_t CYCLE : 6; + // uint16_t HDR_CRC_9_0 : 10; + // } C3xFlex; + // }; + // unsigned char CxTRB0Dall[8]; + // union { + // uint16_t CxTRB0STAT; + // uint16_t J1850_TX_ID; + // }; + // union { + // struct + // { + // uint32_t uiTimeStamp10uS; + // union { + // uint32_t uiTimeStamp10uSMSB; + // struct + // { + // unsigned : 28; + // unsigned res_0s : 3;// must be 0!!! + // unsigned bIsExtended : 1;// always 1 for CoreMiniMsgExtended. + // }; + // }; + // }; + // int64_t uiTimeStampLarge; + // uint8_t uiTimeStampBytes[8]; + // }; + // }; }; } diff --git a/communication/include/packetizer.h b/communication/include/packetizer.h new file mode 100644 index 0000000..b4f7791 --- /dev/null +++ b/communication/include/packetizer.h @@ -0,0 +1,38 @@ +#ifndef __PACKETIZER_H_ +#define __PACKETIZER_H_ + +#include "communication/include/communication.h" +#include +#include +#include + +namespace icsneo { + +class Packetizer { +public: + bool input(const std::vector& bytes); + std::vector> output(); + +private: + enum class ReadState { + SearchForHeader, + ParseHeader, + ParseLongStylePacketHeader, + GetData + }; + ReadState state = ReadState::SearchForHeader; + + int currentIndex = 0; + int packetLength = 0; + int headerSize = 0; + bool checksum = false; + bool gotGoodPackets = false; // Tracks whether we've ever gotten a good packet + Communication::Packet packet; + std::deque bytes; + + std::vector> processedPackets; +}; + +} + +#endif \ No newline at end of file diff --git a/communication/message/include/message.h b/communication/message/include/message.h index fab82c6..39ca828 100644 --- a/communication/message/include/message.h +++ b/communication/message/include/message.h @@ -11,6 +11,7 @@ public: virtual ~Message() {} Network network; std::vector data; + uint64_t timestamp; }; }; diff --git a/communication/messagedecoder.cpp b/communication/messagedecoder.cpp index fd5251a..71efe32 100644 --- a/communication/messagedecoder.cpp +++ b/communication/messagedecoder.cpp @@ -4,136 +4,29 @@ using namespace icsneo; -CANMessage MessageDecoder::CoreMiniMsg::toCANMessage(Network network) { - CANMessage msg; - msg.network = network; - msg.arbid = CxTRB0SID.SID; - msg.data.reserve(CxTRB0DLC.DLC); - for(auto i = 0; i < CxTRB0DLC.DLC; i++) - msg.data.push_back(CxTRB0Dall[i]); - return msg; -} +std::shared_ptr MessageDecoder::decodePacket(const std::shared_ptr& packet) { + switch(packet->network.getType()) { + case Network::Type::CAN: { + if(packet->data.size() < 24) + break; // We would read garbage when interpereting the data -bool MessageDecoder::input(const std::vector& inputBytes) { - bool haveEnoughData = true; - bytes.insert(bytes.end(), inputBytes.begin(), inputBytes.end()); + HardwareCANPacket* data = (HardwareCANPacket*)packet->data.data(); + auto msg = std::make_shared(); + msg->network = packet->network; + msg->arbid = data->header.SID; + msg->data.reserve(data->dlc.DLC); - while(haveEnoughData) { - switch(state) { - case ReadState::SearchForHeader: - if(bytes.size() < 1) { - haveEnoughData = false; - break; - } + // Timestamp calculation + msg->timestamp = data->timestamp.TS * 25; // Timestamps are in 25ns increments since 1/1/2007 GMT 00:00:00.0000 - if(bytes[0] == 0xAA) { // 0xAA denotes the beginning of a packet - state = ReadState::ParseHeader; - currentIndex = 1; - } else { - //std::cout << (int)bytes[0] << " "; - bytes.pop_front(); // Discard - } - break; - case ReadState::ParseHeader: - if(bytes.size() < 2) { - haveEnoughData = false; - break; - } - - messageLength = bytes[1] >> 4 & 0xf; // Upper nibble of the second byte denotes the message length - message.network = Network(bytes[1] & 0xf); // Lower nibble of the second byte is the network ID - if(messageLength == 0) { // A length of zero denotes a long style packet - state = ReadState::ParseLongStylePacketHeader; - checksum = false; - headerSize = 6; - } else { - state = ReadState::GetData; - checksum = true; - headerSize = 2; - messageLength += 2; // The message length given in short messages does not include header - } - currentIndex++; - break; - case ReadState::ParseLongStylePacketHeader: - if(bytes.size() < 6) { - haveEnoughData = false; - break; - } - - messageLength = bytes[2]; // Long messages have a little endian length on bytes 3 and 4 - messageLength |= bytes[3] << 8; - message.network = Network((bytes[5] << 8) | bytes[4]); // Long messages have their netid stored as little endian on bytes 5 and 6 - currentIndex += 4; - - /* Long messages can't have a length less than 4, because that would indicate a negative payload size. - * Unlike the short message length, the long message length encompasses everything from the 0xAA to the - * end of the payload. The short message length, for reference, only encompasses the length of the actual - * payload, and not the header or checksum. - */ - if(messageLength < 4 || messageLength > 4000) { - bytes.pop_front(); - //std::cout << "skipping long message with length " << messageLength << std::endl; - state = ReadState::SearchForHeader; - } else { - state = ReadState::GetData; - } - break; - case ReadState::GetData: - // We do not include the checksum in messageLength so it doesn't get copied into the payload buffer - if(bytes.size() < messageLength + (checksum ? 1 : 0)) { // Read until we have the rest of the message - haveEnoughData = false; - break; - } - - message.data.clear(); - if(messageLength > 0) - message.data.reserve(messageLength - headerSize); - - while(currentIndex < messageLength) - message.data.push_back(bytes[currentIndex++]); - - if(!checksum || bytes[currentIndex] == Communication::ICSChecksum(message.data)) { - // Got a good packet - gotGoodMessages = true; - processMessage(message); - for (auto i = 0; i < messageLength; i++) - bytes.pop_front(); - - } else { - if(gotGoodMessages) // Don't complain unless we've already gotten a good message, in case we started in the middle of a stream - std::cout << "Dropping message due to bad checksum" << std::endl; - bytes.pop_front(); // Drop the first byte so it doesn't get picked up again - } - - // Reset for the next packet - currentIndex = 0; - state = ReadState::SearchForHeader; - break; + for(auto i = 0; i < data->dlc.DLC; i++) + msg->data.push_back(data->data[i]); + return msg; } } - return processedMessages.size() > 0; -} - -std::vector> MessageDecoder::output() { - auto ret = std::move(processedMessages); - processedMessages = std::vector>(); // Reset the vector - return ret; -} - -void MessageDecoder::processMessage(const Message& msg) { - switch(msg.network.getType()) { - case Network::Type::CAN: - if(msg.data.size() >= 24) { - CoreMiniMsg* cmsg = (CoreMiniMsg*)msg.data.data(); - processedMessages.push_back(std::make_shared(cmsg->toCANMessage(msg.network))); - } else { - //std::cout << "bad CAN frame " << msg.data.size() << std::endl; - } - break; - default: - // if(msg.network.getNetID() != Network::NetID::Device) - // std::cout << "Message: " << msg.network << " with data length " << msg.data.size() << std::endl; - processedMessages.push_back(std::make_shared(msg)); - } + auto msg = std::make_shared(); + msg->network = packet->network; + msg->data = packet->data; + return msg; } \ No newline at end of file diff --git a/communication/multichannelcommunication.cpp b/communication/multichannelcommunication.cpp index 96bb354..d7a187b 100644 --- a/communication/multichannelcommunication.cpp +++ b/communication/multichannelcommunication.cpp @@ -1,5 +1,6 @@ #include "communication/include/multichannelcommunication.h" #include "communication/include/messagedecoder.h" +#include "communication/include/packetizer.h" #include #include @@ -30,6 +31,7 @@ void MultiChannelCommunication::readTask() { std::deque usbReadFifo; std::vector readBytes; std::vector payloadBytes; + Packetizer packetizer; MessageDecoder decoder; while(!closing) { @@ -95,25 +97,21 @@ void MultiChannelCommunication::readTask() { continue; } - //std::cout << std::dec << "Got a multichannel message! Size: " << currentCommandLength << std::hex << std::setfill('0') << std::setw(2) << " Cmd: 0x" << (int)currentCommandType << std::endl; for(auto i = 0; i < currentReadIndex; i++) usbReadFifo.pop_front(); payloadBytes.clear(); - payloadBytes.reserve(currentCommandLength); + payloadBytes.resize(currentCommandLength); for(auto i = 0; i < currentCommandLength; i++) { - //std::cout << (int)usbReadFifo[0] << ' '; - payloadBytes.push_back(usbReadFifo[0]); - // if(i % 16 == 15) - // std::cout << std::endl; + payloadBytes[i] = usbReadFifo[0]; usbReadFifo.pop_front(); } - //std::cout << std::dec << std::endl; - if(decoder.input(payloadBytes)) { - for(auto& msg : decoder.output()) { - for(auto& cb : messageCallbacks) { - if(!closing) { // We might have closed while reading or processing + if(packetizer.input(payloadBytes)) { + for(auto& packet : packetizer.output()) { + auto msg = decoder.decodePacket(packet); + for(auto& cb : messageCallbacks) { // We might have closed while reading or processing + if(!closing) { cb.second.callIfMatch(msg); } } diff --git a/communication/packetizer.cpp b/communication/packetizer.cpp new file mode 100644 index 0000000..9e8251a --- /dev/null +++ b/communication/packetizer.cpp @@ -0,0 +1,111 @@ +#include "communication/include/packetizer.h" +#include + +using namespace icsneo; + +bool Packetizer::input(const std::vector& inputBytes) { + bool haveEnoughData = true; + bytes.insert(bytes.end(), inputBytes.begin(), inputBytes.end()); + + while(haveEnoughData) { + switch(state) { + case ReadState::SearchForHeader: + if(bytes.size() < 1) { + haveEnoughData = false; + break; + } + + if(bytes[0] == 0xAA) { // 0xAA denotes the beginning of a packet + state = ReadState::ParseHeader; + currentIndex = 1; + } else { + bytes.pop_front(); // Discard + } + break; + case ReadState::ParseHeader: + if(bytes.size() < 2) { + haveEnoughData = false; + break; + } + + packetLength = bytes[1] >> 4 & 0xf; // Upper nibble of the second byte denotes the packet length + packet.network = Network(bytes[1] & 0xf); // Lower nibble of the second byte is the network ID + if(packetLength == 0) { // A length of zero denotes a long style packet + state = ReadState::ParseLongStylePacketHeader; + checksum = false; + headerSize = 6; + } else { + state = ReadState::GetData; + checksum = true; + headerSize = 2; + packetLength += 2; // The packet length given in short packets does not include header + } + currentIndex++; + break; + case ReadState::ParseLongStylePacketHeader: + if(bytes.size() < 6) { + haveEnoughData = false; + break; + } + + packetLength = bytes[2]; // Long packets have a little endian length on bytes 3 and 4 + packetLength |= bytes[3] << 8; + packet.network = Network((bytes[5] << 8) | bytes[4]); // Long packets have their netid stored as little endian on bytes 5 and 6 + currentIndex += 4; + + /* Long packets can't have a length less than 4, because that would indicate a negative payload size. + * Unlike the short packet length, the long packet length encompasses everything from the 0xAA to the + * end of the payload. The short packet length, for reference, only encompasses the length of the actual + * payload, and not the header or checksum. + */ + if(packetLength < 4 || packetLength > 4000) { + bytes.pop_front(); + //std::cout << "skipping long packet with length " << packetLength << std::endl; + state = ReadState::SearchForHeader; + } else { + state = ReadState::GetData; + } + break; + case ReadState::GetData: + // We do not include the checksum in packetLength so it doesn't get copied into the payload buffer + if(bytes.size() < packetLength + (checksum ? 1 : 0)) { // Read until we have the rest of the packet + haveEnoughData = false; + break; + } + + packet.data.clear(); + if(packetLength > 0) + packet.data.resize(packetLength - headerSize); + + auto i = 0; + while(currentIndex < packetLength) + packet.data[i++] = bytes[currentIndex++]; + + if(!checksum || bytes[currentIndex] == Communication::ICSChecksum(packet.data)) { + // Got a good packet + gotGoodPackets = true; + processedPackets.push_back(std::make_shared(packet)); + for (auto i = 0; i < packetLength; i++) + bytes.pop_front(); + + } else { + if(gotGoodPackets) // Don't complain unless we've already gotten a good packet, in case we started in the middle of a stream + std::cout << "Dropping packet due to bad checksum" << std::endl; + bytes.pop_front(); // Drop the first byte so it doesn't get picked up again + } + + // Reset for the next packet + currentIndex = 0; + state = ReadState::SearchForHeader; + break; + } + } + + return processedPackets.size() > 0; +} + +std::vector> Packetizer::output() { + auto ret = std::move(processedPackets); + processedPackets = std::vector>(); // Reset the vector + return ret; +} \ No newline at end of file