From 5bf50ddc00fcb675b0a1428ab51ed5349947f698 Mon Sep 17 00:00:00 2001 From: Paul Hollinsky Date: Mon, 1 Oct 2018 16:16:49 -0400 Subject: [PATCH] Begin adding an encoder for messages --- communication/communication.cpp | 2 +- communication/encoder.cpp | 61 ++++++++++++++++++ communication/include/communication.h | 2 +- communication/include/encoder.h | 63 +++++++++++++++++++ .../include/multichannelcommunication.h | 6 +- communication/include/packetizer.h | 2 +- communication/multichannelcommunication.cpp | 8 +-- communication/packetizer.cpp | 4 +- 8 files changed, 133 insertions(+), 15 deletions(-) create mode 100644 communication/encoder.cpp create mode 100644 communication/include/encoder.h diff --git a/communication/communication.cpp b/communication/communication.cpp index f7d24cc..7940f79 100644 --- a/communication/communication.cpp +++ b/communication/communication.cpp @@ -46,7 +46,7 @@ bool Communication::close() { } bool Communication::sendPacket(std::vector& bytes) { - return impl->write(packetizer->packetWrap(bytes)); + return rawWrite(packetizer->packetWrap(bytes)); } bool Communication::sendCommand(Command cmd, std::vector arguments) { diff --git a/communication/encoder.cpp b/communication/encoder.cpp new file mode 100644 index 0000000..7740c4d --- /dev/null +++ b/communication/encoder.cpp @@ -0,0 +1,61 @@ +#include "communication/include/encoder.h" + +using namespace icsneo; + +std::shared_ptr Encoder::encode(const std::shared_ptr& message) { + switch(message->network.getType()) { + // case Network::Type::CAN: { + // if(message->data.size() < 24) + // break; // We would read garbage when interpereting the data + + // HardwareCANPacket* data = (HardwareCANPacket*)message->data.data(); + // auto msg = std::make_shared(); + // msg->network = message->network; + // msg->arbid = data->header.SID; + // msg->data.reserve(data->dlc.DLC); + + // // Timestamp calculation + // msg->timestamp = data->timestamp.TS * 25; // Timestamps are in 25ns increments since 1/1/2007 GMT 00:00:00.0000 + + // for(auto i = 0; i < data->dlc.DLC; i++) + // msg->data.push_back(data->data[i]); + // return msg; + // } + default: + switch(message->network.getNetID()) { + case Network::NetID::Main51: + if(message->data.size() > 0xF) { + message->network = Network::NetID::RED_OLDFORMAT; + message->data.insert(message->data.begin(), 0x10 | (uint8_t)Network::NetID::Main51); + return encode(message); + } + message->data.insert(message->data.begin(), (message->data.size() << 4) | (uint8_t)Network::NetID::Main51); + data.in + break; + case Network::NetID::RED_OLDFORMAT: { + // See the decoder for an explanation + uint16_t length = message->data[0] | (message->data[1] << 8); + message->network = Network(message->data[2] & 0xF); + message->data.erase(message->data.begin(), message->data.begin() + 3); + if(message->data.size() != length) + message->data.resize(length); + return decodePacket(message); + } + } + break; + } + + auto msg = std::make_shared(); + msg->network = message->network; + msg->data = message->data; + return msg; +} + +std::vector Encoder::encode(Command cmd, std::vector arguments = {}) { + auto msg = std::make_shared(); + msg->network = Network::NetID::Main51; + msg->data.resize(arguments.size() + 1); + msg->data.push_back((uint8_t)cmd); + msg->data.insert(msg->data.end(), std::make_move_iterator(arguments.begin()), std::make_move_iterator(arguments.end())); + return encode(msg); +} \ No newline at end of file diff --git a/communication/include/communication.h b/communication/include/communication.h index eb24fbf..d8b0a17 100644 --- a/communication/include/communication.h +++ b/communication/include/communication.h @@ -28,7 +28,7 @@ public: virtual void spawnThreads(); virtual void joinThreads(); bool rawWrite(const std::vector& bytes) { return impl->write(bytes); } - bool sendPacket(std::vector& bytes); + virtual bool sendPacket(std::vector& bytes); virtual bool sendCommand(Command cmd, bool boolean) { return sendCommand(cmd, std::vector({ (uint8_t)boolean })); } virtual bool sendCommand(Command cmd, std::vector arguments = {}); diff --git a/communication/include/encoder.h b/communication/include/encoder.h new file mode 100644 index 0000000..e55db16 --- /dev/null +++ b/communication/include/encoder.h @@ -0,0 +1,63 @@ +#ifndef __ENCODER_H_ +#define __ENCODER_H_ + +#include "communication/message/include/message.h" +#include "communication/message/include/canmessage.h" +#include "communication/include/packet.h" +#include "communication/include/network.h" +#include +#include +#include + +#pragma pack(push, 1) + +namespace icsneo { + +class Encoder { +public: + std::vector encode(const std::shared_ptr& message); + std::vector encode(Command cmd, bool boolean) { return encode(cmd, std::vector({ (uint8_t)boolean })); } + std::vector encode(Command cmd, std::vector arguments = {}); + +private: + typedef uint16_t icscm_bitfield; + 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; + }; +}; + +} + +#pragma pack(pop) + +#endif \ No newline at end of file diff --git a/communication/include/multichannelcommunication.h b/communication/include/multichannelcommunication.h index db15814..fb23f99 100644 --- a/communication/include/multichannelcommunication.h +++ b/communication/include/multichannelcommunication.h @@ -10,9 +10,9 @@ namespace icsneo { class MultiChannelCommunication : public Communication { public: MultiChannelCommunication(std::shared_ptr com, std::shared_ptr p, std::shared_ptr md) : Communication(com, p, md) {} - void spawnThreads(); - void joinThreads(); - bool sendCommand(Command cmd, std::vector arguments); + void spawnThreads() override; + void joinThreads() override; + bool sendPacket(std::vector& bytes) override; protected: bool preprocessPacket(std::deque& usbReadFifo); diff --git a/communication/include/packetizer.h b/communication/include/packetizer.h index 1c773e6..234a17b 100644 --- a/communication/include/packetizer.h +++ b/communication/include/packetizer.h @@ -11,7 +11,7 @@ namespace icsneo { class Packetizer { public: static uint8_t ICSChecksum(const std::vector& data); - std::vector& packetWrap(std::vector& data); + std::vector& packetWrap(std::vector& data, bool checksum = true); bool input(const std::vector& bytes); std::vector> output(); diff --git a/communication/multichannelcommunication.cpp b/communication/multichannelcommunication.cpp index 01d7367..00c5cf0 100644 --- a/communication/multichannelcommunication.cpp +++ b/communication/multichannelcommunication.cpp @@ -16,13 +16,7 @@ void MultiChannelCommunication::joinThreads() { mainChannelReadThread.join(); } -bool MultiChannelCommunication::sendCommand(Command cmd, std::vector arguments) { - std::vector bytes; - bytes.push_back((uint8_t)cmd); - for(auto& b : arguments) - bytes.push_back(b); - bytes.insert(bytes.begin(), 0xB | ((uint8_t)bytes.size() << 4)); - bytes = packetizer->packetWrap(bytes); +bool MultiChannelCommunication::sendPacket(std::vector& bytes) { bytes.insert(bytes.begin(), {(uint8_t)CommandType::HostPC_to_Vnet1, (uint8_t)bytes.size(), (uint8_t)(bytes.size() >> 8)}); return rawWrite(bytes); } diff --git a/communication/packetizer.cpp b/communication/packetizer.cpp index 1e7cd7b..d024156 100644 --- a/communication/packetizer.cpp +++ b/communication/packetizer.cpp @@ -12,8 +12,8 @@ uint8_t Packetizer::ICSChecksum(const std::vector& data) { return (uint8_t)checksum; } -std::vector& Packetizer::packetWrap(std::vector& data) { - if(!disableChecksum) +std::vector& Packetizer::packetWrap(std::vector& data, bool checksum) { + if(!disableChecksum && checksum) data.push_back(ICSChecksum(data)); data.insert(data.begin(), 0xAA); if(align16bit && data.size() % 2 == 1)