diff --git a/communication/encoder.cpp b/communication/encoder.cpp index 7848dd3..33d9a90 100644 --- a/communication/encoder.cpp +++ b/communication/encoder.cpp @@ -2,6 +2,7 @@ #include "icsneo/communication/message/ethernetmessage.h" #include "icsneo/communication/message/main51message.h" #include "icsneo/communication/packet/ethernetpacket.h" +#include "icsneo/communication/packet/iso9141packet.h" #include "icsneo/communication/packet/canpacket.h" using namespace icsneo; @@ -45,6 +46,17 @@ bool Encoder::encode(const Packetizer& packetizer, std::vector& result, break; } // End of Network::Type::CAN + case Network::Type::ISO9141: { + auto isomsg = std::dynamic_pointer_cast(message); + if(!isomsg) { + report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error); + return false; // The message was not a properly formed ISO9141Message + } + + // Skip the normal message wrapping at the bottom since we need to send multiple + // packets to the device. This function just encodes them back to back into `result` + return HardwareISO9141Packet::EncodeFromMessage(*isomsg, result, report, packetizer); + } // End of Network::Type::ISO9141 default: switch(message->network.getNetID()) { case Network::NetID::Device: @@ -91,6 +103,7 @@ bool Encoder::encode(const Packetizer& packetizer, std::vector& result, } } + // Early returns may mean we don't reach this far, check the type you're concerned with auto& buffer = useResultAsBuffer ? result : message->data; if(shortFormat) { diff --git a/communication/packet/iso9141packet.cpp b/communication/packet/iso9141packet.cpp index 56438ec..2904ae9 100644 --- a/communication/packet/iso9141packet.cpp +++ b/communication/packet/iso9141packet.cpp @@ -1,8 +1,79 @@ #include "icsneo/communication/packet/iso9141packet.h" +#include "icsneo/communication/packetizer.h" #include using namespace icsneo; +bool HardwareISO9141Packet::EncodeFromMessage(const ISO9141Message& message, std::vector& bytestream, + const device_eventhandler_t& report, const Packetizer& packetizer) +{ + size_t bytesToSend = message.data.size(); + if (message.isInit || message.isBreak) + bytesToSend = 0; + + if(bytesToSend > 4200) { + report(APIEvent::Type::MessageMaxLengthExceeded, APIEvent::Severity::Error); + return false; // Too much data for the protocol + } + + bytestream.clear(); + + std::vector packet; + packet.reserve(16); + + size_t currentStart = 0; + do { + const bool firstPacket = currentStart == 0; + const uint8_t maxSize = (firstPacket ? 9 : 12); + uint8_t currentSize = maxSize; + if(bytesToSend - currentStart < maxSize) + currentSize = bytesToSend - currentStart; + + packet.insert(packet.begin(), { + (uint8_t)Network::NetID::RED, // 0x0C for long message + (uint8_t)0, // Size, little endian 16-bit, filled later + (uint8_t)0, + (uint8_t)message.network.getNetID(), // NetID, little endian 16-bit + (uint8_t)(uint16_t(message.network.getNetID()) >> 8) + }); + packet.push_back(uint8_t(message.network.getNetID()) + uint8_t((currentSize + (firstPacket ? 6 : 3)) << 4)); + packet.push_back(uint8_t(currentSize + (firstPacket ? 5 : 2))); + if(bytesToSend - currentStart > maxSize) // More packets are coming + packet.back() |= 0x40; + + if(firstPacket) { + if(message.isInit) + packet.back() |= 0x80; + if(message.isBreak) + packet.back() |= 0x20; + } + + // Two bytes for Description ID, big endian + packet.insert(packet.end(), { uint8_t(message.description >> 8), uint8_t(message.description) }); + + // If we're the first packet and not init/break only, we should put the header in + if(firstPacket && !message.isInit && !message.isBreak) + packet.insert(packet.end(), message.header.begin(), message.header.end()); + + // Now the data + auto dataIt = message.data.begin() + currentStart; + if(currentSize) + packet.insert(packet.end(), dataIt, dataIt + currentSize); + + // Advance for the next packet + currentStart += currentSize; + + const uint16_t size = uint16_t(packet.size()) + 2; + packet[1] = uint8_t(size & 0xFF); + packet[2] = uint8_t((size >> 8) & 0xFF); + + packetizer.packetWrap(packet, false); + bytestream.insert(bytestream.end(), packet.begin(), packet.end()); + packet.clear(); + } while(currentStart < bytesToSend); + return true; +} + std::shared_ptr HardwareISO9141Packet::Decoder::decodeToMessage(const std::vector& bytestream) { const HardwareISO9141Packet* data = (const HardwareISO9141Packet*)bytestream.data(); diff --git a/examples/cpp/simple/src/SimpleExample.cpp b/examples/cpp/simple/src/SimpleExample.cpp index e840b9a..68f4e87 100644 --- a/examples/cpp/simple/src/SimpleExample.cpp +++ b/examples/cpp/simple/src/SimpleExample.cpp @@ -182,7 +182,7 @@ int main() { // A message of type ISO9414 is guaranteed to be an ISO9141Message, so we can static cast safely auto isoMessage = std::static_pointer_cast(message); - std::cout << "\t\tISO 9141-2 "; + std::cout << "\t\t" << isoMessage->network << ' '; // Print the header bytes std::cout << '(' << std::hex << std::setfill('0') << std::setw(2) << (uint32_t)isoMessage->header[0] << ' '; diff --git a/include/icsneo/communication/message/iso9141message.h b/include/icsneo/communication/message/iso9141message.h index bd091d3..e994f59 100644 --- a/include/icsneo/communication/message/iso9141message.h +++ b/include/icsneo/communication/message/iso9141message.h @@ -12,6 +12,7 @@ class ISO9141Message : public Message { public: std::array header; bool isInit = false; + bool isBreak = false; bool framingError = false; bool overflowError = false; bool parityError = false; diff --git a/include/icsneo/communication/packet/iso9141packet.h b/include/icsneo/communication/packet/iso9141packet.h index ae8b3aa..0d7bcf1 100644 --- a/include/icsneo/communication/packet/iso9141packet.h +++ b/include/icsneo/communication/packet/iso9141packet.h @@ -12,8 +12,12 @@ namespace icsneo { typedef uint16_t icscm_bitfield; +class Packetizer; struct HardwareISO9141Packet { + static bool EncodeFromMessage(const ISO9141Message& message, std::vector& bytestream, + const device_eventhandler_t& report, const Packetizer& packetizer); + class Decoder { public: std::shared_ptr decodeToMessage(const std::vector& bytestream); diff --git a/include/icsneo/device/tree/neovifire2/neovifire2.h b/include/icsneo/device/tree/neovifire2/neovifire2.h index 87b03e9..f10fa7a 100644 --- a/include/icsneo/device/tree/neovifire2/neovifire2.h +++ b/include/icsneo/device/tree/neovifire2/neovifire2.h @@ -36,7 +36,12 @@ public: Network::NetID::LIN, Network::NetID::LIN2, Network::NetID::LIN3, - Network::NetID::LIN4 + Network::NetID::LIN4, + + Network::NetID::ISO9141, + Network::NetID::ISO9141_2, + Network::NetID::ISO9141_3, + Network::NetID::ISO9141_4 }; return supportedNetworks; }