Communication: Add SPI support

communication/decoder.cpp

@@ -43,6 +43,7 @@
 #include "icsneo/communication/packet/genericbinarystatuspacket.h"
 #include "icsneo/communication/packet/livedatapacket.h"
 #include "icsneo/communication/packet/hardwareinfopacket.h"
+#include "icsneo/communication/packet/spipacket.h"
 
 #include <iostream>
 
@@ -184,6 +185,19 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 			msg.timestamp *= timestampResolution;
 			return true;
 		}
+		case Network::Type::SPI: {
+			result = HardwareSPIPacket::DecodeToMessage(packet->data);
+
+			if(!result) {
+				report(APIEvent::Type::PacketDecodingError, APIEvent::Severity::Error);
+				return false; // A nullptr was returned, the packet was not long enough to decode
+			}
+
+			SPIMessage& msg = *static_cast<SPIMessage*>(result.get());
+			msg.network = packet->network;
+			msg.timestamp *= timestampResolution;
+			return true;
+		}
 		case Network::Type::MDIO: {
 			result = HardwareMDIOPacket::DecodeToMessage(packet->data);
 

communication/encoder.cpp

@@ -13,6 +13,7 @@
 #include "icsneo/communication/packet/a2bpacket.h"
 #include "icsneo/communication/packet/linpacket.h"
 #include "icsneo/communication/packet/mdiopacket.h"
+#include "icsneo/communication/packet/spipacket.h"
 
 using namespace icsneo;
 
@@ -133,6 +134,17 @@ bool Encoder::encode(const Packetizer& packetizer, std::vector<uint8_t>& result,
 					}
 					break;
 				} // End of Network::Type::MDIO
+				case Network::Type::SPI: {
+					auto msg = std::dynamic_pointer_cast<SPIMessage>(message);
+					if(!msg) {
+						report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error);
+						return false; // The message was not a properly formed LiveDataMessage
+					}
+					if(!HardwareSPIPacket::EncodeFromMessage(*msg, result, report))
+						return false;
+					result = packetizer.packetWrap(result, false);
+					return true;
+				} // End of Network::Type::SPI
 				default:
 					report(APIEvent::Type::UnexpectedNetworkType, APIEvent::Severity::Error);
 					return false;

communication/packet/spipacket.cpp

@@ -0,0 +1,82 @@
+#include "icsneo/communication/packet/spipacket.h"
+#include "icsneo/communication/command.h"
+#include <cstring>
+#include <vector>
+
+using namespace icsneo;
+
+static size_t SPISubHeaderLength = 5u;
+
+std::shared_ptr<Message> HardwareSPIPacket::DecodeToMessage(const std::vector<uint8_t>& bytestream) {
+	if(bytestream.size() < sizeof(HardwareSPIPacket)) {
+		return nullptr;
+	}
+	const HardwareSPIPacket* packet = (const HardwareSPIPacket*)bytestream.data();
+	size_t totalPackedLength = static_cast<size_t>(bytestream.size()) - sizeof(HardwareSPIPacket); // First 28 bytes are message header.
+	if(totalPackedLength < SPISubHeaderLength) {
+		return nullptr;
+	}
+	
+	const uint8_t* bytes = bytestream.data() + sizeof(HardwareSPIPacket);
+	std::shared_ptr<SPIMessage> msg = std::make_shared<SPIMessage>();
+	msg->direction = static_cast<SPIMessage::Direction>(bytes[0]);
+	msg->address = *reinterpret_cast<const uint16_t*>(&bytes[1]);
+	msg->mms = bytes[3];
+	msg->stats = packet->stats;
+	msg->timestamp = packet->timestamp.TS;
+	
+	size_t numWords = (totalPackedLength - SPISubHeaderLength) / 4;
+	msg->payload.reserve(numWords);
+	for(size_t offset = SPISubHeaderLength; offset < totalPackedLength; offset += 4) {
+		msg->payload.push_back(*reinterpret_cast<const uint32_t*>(bytes + offset));
+	}
+	return msg;
+}
+
+bool HardwareSPIPacket::EncodeFromMessage(const SPIMessage& message, std::vector<uint8_t>& bytestream, const device_eventhandler_t& /*report*/) {
+	// Payload length is everything excluding cmdHeader (note at the beginning there is an offset of 2)
+	uint16_t payloadLength = static_cast<uint16_t>(
+		2 + 
+		sizeof(HardwareSPIPacket) + 
+		SPISubHeaderLength + 
+		message.payload.size() * sizeof(uint32_t)
+	);
+	if(payloadLength % 2) {
+		// Pad payload to even number
+		payloadLength++;
+	}
+	// +1 for AA, another +1 for firmware nuance
+	uint16_t fullSize = 1 + sizeof(ExtendedCommandHeader) + payloadLength + 1;
+	uint16_t unwrappedSize = sizeof(ExtendedCommandHeader) + payloadLength; // fullSize without AA and firmware nuance
+
+	bytestream.resize(unwrappedSize, 0);
+	uint32_t offset = 0;
+	auto* cmdHeader = reinterpret_cast<ExtendedCommandHeader*>(bytestream.data() + offset);
+	cmdHeader->netid = static_cast<uint8_t>(Network::NetID::Main51);
+	cmdHeader->fullLength = fullSize;
+	cmdHeader->command = static_cast<uint8_t>(Command::Extended);
+	cmdHeader->extendedCommand = static_cast<uint16_t>(ExtendedCommand::TransmitCoreminiMessage);
+	cmdHeader->payloadLength = payloadLength;
+	
+	offset += sizeof(ExtendedCommandHeader) + 2; // Offset of 2 between header and packet
+	auto* packet = reinterpret_cast<HardwareSPIPacket*>(bytestream.data() + offset);
+	packet->header.frameLength = static_cast<uint16_t>(SPISubHeaderLength + message.payload.size() * sizeof(uint32_t));
+	packet->networkID = static_cast<uint16_t>(message.network.getNetID());
+	packet->length = packet->header.frameLength;
+	packet->timestamp.IsExtended = 1;
+
+	offset += sizeof(HardwareSPIPacket);
+	// Write the sub header details
+	bytestream[offset++] = static_cast<uint8_t>(message.direction);
+	bytestream[offset++] = static_cast<uint8_t>(message.address & 0xFF);
+	bytestream[offset++] = static_cast<uint8_t>((message.address >> 8) & 0xFF);
+	bytestream[offset++] = static_cast<uint8_t>(message.mms);
+	bytestream[offset++] = static_cast<uint8_t>(message.payload.size());
+
+	// Write the words
+	for(uint32_t word : message.payload) {
+		*reinterpret_cast<uint32_t*>(bytestream.data() + offset) = word;
+		offset += sizeof(uint32_t);
+	}
+	return true;
+}