Begin work on FlexRay support

communication/communication.cpp

@@ -141,6 +141,22 @@ std::shared_ptr<Message> Communication::waitForMessageSync(std::shared_ptr<Messa
 	return returnedMessage;
 }
 
+void Communication::dispatchMessage(const std::shared_ptr<Message>& msg) {
+	std::lock_guard<std::mutex> lk(messageCallbacksLock);
+
+	// We want callbacks to be able to access errors
+	const bool downgrade = EventManager::GetInstance().isDowngradingErrorsOnCurrentThread();
+	if(downgrade)
+		EventManager::GetInstance().cancelErrorDowngradingOnCurrentThread();
+	for(auto& cb : messageCallbacks) {
+		if(!closing) { // We might have closed while reading or processing
+			cb.second.callIfMatch(msg);
+		}
+	}
+	if(downgrade)
+		EventManager::GetInstance().downgradeErrorsOnCurrentThread();
+}
+
 void Communication::readTask() {
 	std::vector<uint8_t> readBytes;
 
@@ -155,17 +171,7 @@ void Communication::readTask() {
 					if(!decoder->decode(msg, packet))
 						continue;
 
-					std::lock_guard<std::mutex> lk(messageCallbacksLock);
-
-					for(auto& cb : messageCallbacks) {
-						if(!closing) { // We might have closed while reading or processing
-							// We want callbacks to be able to access errors
-							EventManager::GetInstance().cancelErrorDowngradingOnCurrentThread();
-							cb.second.callIfMatch(msg);
-							EventManager::GetInstance().downgradeErrorsOnCurrentThread();
-						}
-					}
-					
+					dispatchMessage(msg);
 				}
 			}
 		}

communication/decoder.cpp

@@ -3,10 +3,12 @@
 #include "icsneo/communication/message/serialnumbermessage.h"
 #include "icsneo/communication/message/resetstatusmessage.h"
 #include "icsneo/communication/message/readsettingsmessage.h"
+#include "icsneo/communication/message/flexray/control/flexraycontrolmessage.h"
 #include "icsneo/communication/command.h"
 #include "icsneo/device/device.h"
 #include "icsneo/communication/packet/canpacket.h"
 #include "icsneo/communication/packet/ethernetpacket.h"
+#include "icsneo/communication/packet/flexraypacket.h"
 #include <iostream>
 
 using namespace icsneo;
@@ -51,6 +53,23 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 			result->network = packet->network;
 			return true;
 		}
+		case Network::Type::FlexRay: {
+			if(packet->data.size() < 24) {
+				report(APIEvent::Type::PacketDecodingError, APIEvent::Severity::Error);
+				return false;
+			}
+
+			result = HardwareFlexRayPacket::DecodeToMessage(packet->data);
+			if(!result) {
+				report(APIEvent::Type::PacketDecodingError, APIEvent::Severity::Error);
+				return false; // A nullptr was returned, the packet was malformed
+			}
+			// Timestamps are in (resolution) ns increments since 1/1/2007 GMT 00:00:00.0000
+			// The resolution depends on the device
+			result->timestamp *= timestampResolution;
+			result->network = packet->network;
+			return true;
+		}
 		case Network::Type::Internal: {
 			switch(packet->network.getNetID()) {
 				case Network::NetID::Reset_Status: {
@@ -82,6 +101,15 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 					result = msg;
 					return true;
 				}
+				case Network::NetID::FlexRayControl: {
+					auto frResult = std::make_shared<FlexRayControlMessage>(*packet);
+					if(!frResult->decoded) {
+						report(APIEvent::Type::PacketDecodingError, APIEvent::Severity::Error);
+						return false;
+					}
+					result = frResult;
+					return true;
+				}
 				default:
 					break;//return false;
 			}

communication/message/flexray/control/flexraycontrolmessage.cpp

@@ -0,0 +1,70 @@
+#include <icsneo/communication/message/flexray/control/flexraycontrolmessage.h>
+#include <cstring> // memcpy
+
+using namespace icsneo;
+
+std::vector<uint8_t> FlexRayControlMessage::BuildBaseControlArgs(uint8_t controller, FlexRay::Opcode op, std::initializer_list<uint8_t> args) {
+	std::vector<uint8_t> ret;
+	ret.reserve(args.size() + 4);
+	ret.push_back(controller);
+	const uint16_t size = args.size() + 1; // Add 1 for the opcode
+	ret.push_back(uint8_t(size));
+	ret.push_back(uint8_t(size >> 8));
+	ret.push_back(uint8_t(op));
+	ret.insert(ret.end(), args.begin(), args.end());
+	return ret;
+}
+
+std::vector<uint8_t> FlexRayControlMessage::BuildReadCCRegsArgs(uint8_t controller, uint16_t startAddress, uint8_t numRegisters) {
+	startAddress /= 4;
+	return BuildBaseControlArgs(controller, FlexRay::Opcode::ReadCCRegs, {
+		uint8_t(startAddress),
+		uint8_t(startAddress >> 8),
+		numRegisters
+	});
+}
+
+std::vector<uint8_t> FlexRayControlMessage::BuildWriteCCRegArgs(uint8_t controller, uint16_t address, uint32_t value) {
+	address /= 4;
+	return BuildBaseControlArgs(controller, FlexRay::Opcode::ReadCCRegs, {
+		uint8_t(address),
+		uint8_t(address >> 8),
+		uint8_t(value),
+		uint8_t(value >> 8),
+		uint8_t(value >> 16),
+		uint8_t(value >> 24)
+	});
+}
+
+FlexRayControlMessage::FlexRayControlMessage(const Packet& packet) : Message() {
+	if(packet.data.size() < 2)
+		return; // huh?
+	controller = packet.data[0];
+	if(controller < 2)
+		return; // Invalid controller
+
+	// Opcode is only ReadCCStatus or ReadCCRegs for the moment
+	opcode = FlexRay::Opcode(packet.data[1]);
+	if(opcode != FlexRay::Opcode::ReadCCRegs && opcode != FlexRay::Opcode::ReadCCStatus)
+		return;
+	
+	// Read out registers
+	size_t bytes = packet.data.size() - 2;
+	const size_t count = bytes / sizeof(uint32_t);
+	bytes -= bytes % sizeof(uint32_t); // trim off any trailing bytes
+	registers.resize(count);
+	memcpy(registers.data(), packet.data.data() + 2, bytes);
+
+	// If it was a status message, we should decode these registers into their components
+	if(opcode == FlexRay::Opcode::ReadCCStatus) {
+		if(count < 8)
+			return;
+		pocStatus = FlexRay::POCStatus(registers[0] & 0x0000003F);
+		slotCounterA = registers[4] & 0x0000FFFF;
+		slotCounterB = (registers[4] & 0xFFFF0000) >> 16;
+		rateCorrection = registers[6];
+		offsetCorrection = registers[7];
+	}
+
+	decoded = true;
+}

communication/multichannelcommunication.cpp

@@ -25,6 +25,7 @@ bool MultiChannelCommunication::sendPacket(std::vector<uint8_t>& bytes) {
 
 void MultiChannelCommunication::readTask() {
 	bool readMore = true;
+	bool gotPacket = false; // Have we got the first valid packet (don't flag errors otherwise)
 	std::deque<uint8_t> usbReadFifo;
 	std::vector<uint8_t> readBytes;
 	std::vector<uint8_t> payloadBytes;
@@ -108,19 +109,11 @@ void MultiChannelCommunication::readTask() {
 					if(packetizer->input(payloadBytes)) {
 						for(auto& packet : packetizer->output()) {
 							std::shared_ptr<Message> msg;
-							if(!decoder->decode(msg, packet)) {
-								report(APIEvent::Type::Unknown, APIEvent::Severity::Error); // TODO Use specific error
-								continue;
-							}
+							if(!decoder->decode(msg, packet))
+								continue; // Error will have been reported from within decoder
 
-							for(auto& cb : messageCallbacks) { // We might have closed while reading or processing
-								if(!closing) {
-									// We want callbacks to be able to access errors
-									EventManager::GetInstance().cancelErrorDowngradingOnCurrentThread();
-									cb.second.callIfMatch(msg);
-									EventManager::GetInstance().downgradeErrorsOnCurrentThread();
-								}
-							}
+							gotPacket = true;
+							dispatchMessage(msg);
 						}
 					}
 

communication/packet/flexraypacket.cpp

@@ -0,0 +1,63 @@
+#include "icsneo/communication/packet/flexraypacket.h"
+
+using namespace icsneo;
+
+std::shared_ptr<FlexRayMessage> HardwareFlexRayPacket::DecodeToMessage(const std::vector<uint8_t>& bytestream) {
+	const HardwareFlexRayPacket* data = (const HardwareFlexRayPacket*)bytestream.data();
+	if(!data->timestamp.IsExtended) // We can only process "extended" frames here
+		return nullptr;
+
+	auto msg = std::make_shared<FlexRayMessage>();
+
+	// This timestamp is raw off the device (in timestampResolution increments)
+	// Decoder will fix as it has information about the timestampResolution increments
+	msg->timestamp = data->timestamp.TS;
+
+	// Always get the frame length, even for a symbol
+	msg->framelen = data->frame_length_12_5ns * 12.5e-9;
+
+	if(data->tss_length_12_5ns == 0xffff) {// Flag value meaning this is a symbol
+		msg->symbol = FlexRay::Symbol::Unknown; // We can't know the symbol yet because this will depend on the baudrate
+		// Eventually we'll have to get this from the framelen
+	} else {
+		msg->tsslen = data->tss_length_12_5ns * 12.5e-9;
+		msg->channelB = data->statusBits.bits.chb;
+		
+		if(data->statusBits.bits.bytesRxed >= 5) {
+			if(data->statusBits.bits.hcrc_error)
+				msg->headerCRCStatus = FlexRay::CRCStatus::Error;
+		} else {
+			msg->headerCRCStatus = FlexRay::CRCStatus::NoCRC;
+		}
+
+		uint32_t numBytes = data->payload_len * 2;
+		if(ssize_t(numBytes) >= ssize_t(data->Length) - 4) {
+			if(data->statusBits.bits.fcrc_error)
+				msg->crcStatus = FlexRay::CRCStatus::Error;
+		} else {
+			msg->crcStatus = FlexRay::CRCStatus::NoCRC;
+		}
+
+		if(data->statusBits.bits.bytesRxed >= 5) { // Received entire header
+			msg->headerCRC = (data->hdr_crc_10 << 10) | data->hdr_crc_9_0;
+			if(msg->headerCRCStatus != FlexRay::CRCStatus::Error) {
+				msg->reserved0was1 = data->reserved_0;
+				msg->payloadPreamble = data->payload_preamble;
+				msg->nullFrame = data->null_frame;
+				msg->sync = data->sync;
+				msg->startup = data->startup;
+				msg->id = data->id;
+				if(ssize_t(numBytes) != ssize_t(data->Length) - 4) {
+					
+				} else {
+					// This is an error, probably need to flag it
+				}
+			}
+		}
+	}
+	return msg;
+}
+
+bool HardwareFlexRayPacket::EncodeFromMessage(const FlexRayMessage& message, std::vector<uint8_t>& bytestream, const device_eventhandler_t& report) {
+	return false;
+}