Add transmit support for ISO 9141-2

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<uint8_t>& result,
 
 			break;
 		} // End of Network::Type::CAN
+		case Network::Type::ISO9141: {
+			auto isomsg = std::dynamic_pointer_cast<ISO9141Message>(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<uint8_t>& 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) {

communication/packet/iso9141packet.cpp

@@ -1,8 +1,79 @@
 #include "icsneo/communication/packet/iso9141packet.h"
+#include "icsneo/communication/packetizer.h"
 #include <algorithm>
 
 using namespace icsneo;
 
+bool HardwareISO9141Packet::EncodeFromMessage(const ISO9141Message& message, std::vector<uint8_t>& 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<uint8_t> 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<ISO9141Message> HardwareISO9141Packet::Decoder::decodeToMessage(const std::vector<uint8_t>& bytestream) {
 	const HardwareISO9141Packet* data = (const HardwareISO9141Packet*)bytestream.data();