Implement FlexRay transmit, configuration, and cold start

communication/encoder.cpp

@@ -1,5 +1,6 @@
 #include "icsneo/communication/encoder.h"
 #include "icsneo/communication/message/ethernetmessage.h"
+#include "icsneo/communication/message/main51message.h"
 #include "icsneo/communication/packet/ethernetpacket.h"
 #include "icsneo/communication/packet/canpacket.h"
 
@@ -49,24 +50,34 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
 				case Network::NetID::Device:
 					shortFormat = true;
 					break;
-				case Network::NetID::Main51:
+				case Network::NetID::Main51: {
-					if(message->data.size() > 0xF) {
+					auto m51msg = std::dynamic_pointer_cast<Main51Message>(message);
+					if(!m51msg) {
+						report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error);
+						return false; // The message was not a properly formed Main51Message
+					}
+
+					if(!m51msg->forceShortFormat) {
 						// Main51 can be sent as a long message without setting the NetID to RED first
 						// Size in long format is the size of the entire packet
 						// So +1 for AA header, +1 for short format header, and +2 for long format size
 						uint16_t size = uint16_t(message->data.size()) + 1 + 1 + 2;
 						size += 1; // Even though we are not including the NetID bytes, the device expects them to be counted in the length
+						size += 1; // Main51 Command
 						message->data.insert(message->data.begin(), {
 							(uint8_t)Network::NetID::Main51, // 0x0B for long message
 							(uint8_t)size, // Size, little endian 16-bit
-							(uint8_t)(size >> 8)
+							(uint8_t)(size >> 8),
+							(uint8_t)m51msg->command
 						});
 						result = packetizer->packetWrap(message->data, shortFormat);
 						return true;
 					} else {
+						message->data.insert(message->data.begin(), { uint8_t(m51msg->command) });
 						shortFormat = true;
 					}
 					break;
+				}
 				case Network::NetID::RED_OLDFORMAT: {
 					// See the decoder for an explanation
 					// We expect the network byte to be populated already in data, but not the length
@@ -102,14 +113,15 @@ bool Encoder::encode(std::vector<uint8_t>& result, const std::shared_ptr<Message
 }
 
 bool Encoder::encode(std::vector<uint8_t>& result, Command cmd, std::vector<uint8_t> arguments) {
-	auto msg = std::make_shared<Message>();
+	std::shared_ptr<Message> msg;
 	if(cmd == Command::UpdateLEDState) {
 		/* NetID::Device is a super old command type.
 		 * It has a leading 0x00 byte, a byte for command, and a byte for an argument.
 		 * In this case, command 0x06 is SetLEDState.
 		 * This old command type is not really used anywhere else.
 		 */
-		if (arguments.empty()) {
+		msg = std::make_shared<Message>();
+		if(arguments.empty()) {
 			report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error);
 			return false;
 		}
@@ -119,9 +131,20 @@ bool Encoder::encode(std::vector<uint8_t>& result, Command cmd, std::vector<uint
 		msg->data.push_back(0x06); // SetLEDState
 		msg->data.push_back(arguments.at(0)); // See Device::LEDState
 	} else {
+		auto m51msg = std::make_shared<Main51Message>();
+		msg = m51msg;
 		msg->network = Network::NetID::Main51;
-		msg->data.reserve(arguments.size() + 1);
+		m51msg->command = cmd;
-		msg->data.push_back((uint8_t)cmd);
+		switch(cmd) {
+			case Command::RequestSerialNumber:
+			case Command::EnableNetworkCommunication:
+			case Command::EnableNetworkCommunicationEx:
+				// There is a firmware handling idiosyncrasy with these commands
+				// They must be encoded in the short format
+				m51msg->forceShortFormat = true;
+			default:
+				break;
+		}
 		msg->data.insert(msg->data.end(), std::make_move_iterator(arguments.begin()), std::make_move_iterator(arguments.end()));
 	}
 	

communication/message/flexray/control/flexraycontrolmessage.cpp

@@ -2,10 +2,11 @@
 #include <cstring> // memcpy
 #include <limits>
 #include <algorithm>
+#include <iostream>
 
 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> FlexRayControlMessage::BuildBaseControlArgs(uint8_t controller, FlexRay::Opcode op, const std::vector<uint8_t>& args) {
 	std::vector<uint8_t> ret;
 	ret.reserve(args.size() + 4);
 	ret.push_back(controller);
@@ -28,7 +29,7 @@ std::vector<uint8_t> FlexRayControlMessage::BuildReadCCRegsArgs(uint8_t controll
 
 std::vector<uint8_t> FlexRayControlMessage::BuildWriteCCRegArgs(uint8_t controller, uint16_t address, uint32_t value) {
 	address /= 4;
-	return BuildBaseControlArgs(controller, FlexRay::Opcode::ReadCCRegs, {
+	return BuildBaseControlArgs(controller, FlexRay::Opcode::WriteCCReg, {
 		uint8_t(address),
 		uint8_t(address >> 8),
 		uint8_t(value),
@@ -50,11 +51,26 @@ std::vector<uint8_t> FlexRayControlMessage::BuildAddConfiguredTxMessageArgs(
 	});
 }
 
+std::vector<uint8_t> FlexRayControlMessage::BuildWriteMessageBufferArgs(
+	uint8_t controller, uint16_t bufferId, const std::vector<uint8_t>& data, uint16_t desiredSize) {
+	desiredSize += desiredSize % 4; // Must be a multiple of 4
+	std::vector<uint8_t> args = {
+		uint8_t(bufferId),
+		uint8_t(desiredSize / 4)
+	};
+	args.insert(args.end(), data.begin(), data.end());
+	if(args.size() != desiredSize + 2)
+		args.resize(desiredSize + 2);
+	return BuildBaseControlArgs(controller, FlexRay::Opcode::WriteMessageBuffer, args);
+}
+
 FlexRayControlMessage::FlexRayControlMessage(const Packet& packet) : Message() {
+	network = Network::NetID::FlexRayControl;
+
 	if(packet.data.size() < 2)
 		return; // huh?
 	controller = packet.data[0];
-	if(controller < 2)
+	if(controller >= 2)
 		return; // Invalid controller
 
 	// Opcode is only ReadCCStatus or ReadCCRegs for the moment

device/device.cpp

@@ -197,6 +197,7 @@ bool Device::open() {
 		handleInternalMessage(message);
 	}));
 
+	forEachExtension([](const std::shared_ptr<DeviceExtension>& ext) { ext->onDeviceOpen(); return true; });
 	return true;
 }
 
@@ -213,7 +214,8 @@ bool Device::close() {
 		com->removeMessageCallback(internalHandlerCallbackID);
 
 	internalHandlerCallbackID = 0;
-	
+
+	forEachExtension([](const std::shared_ptr<DeviceExtension>& ext) { ext->onDeviceClose(); return true; });
 	return com->close();
 }
 
@@ -242,7 +244,8 @@ bool Device::goOnline() {
 	}
 	
 	online = true;
-	
+
+	forEachExtension([](const std::shared_ptr<DeviceExtension>& ext) { ext->onGoOnline(); return true; });
 	return true;
 }
 
@@ -272,6 +275,7 @@ bool Device::goOffline() {
 	
 	online = false;
 
+	forEachExtension([](const std::shared_ptr<DeviceExtension>& ext) { ext->onGoOffline(); return true; });
 	return true;
 }
 
@@ -293,6 +297,16 @@ bool Device::transmit(std::shared_ptr<Message> message) {
 		return false;
 	}
 
+	bool extensionHookedTransmit = false;
+	bool transmitStatusFromExtension = false;
+	forEachExtension([&](const std::shared_ptr<DeviceExtension>& ext) {
+		if(!ext->transmitHook(message, transmitStatusFromExtension))
+			extensionHookedTransmit = true;
+		return !extensionHookedTransmit; // false breaks out of the loop early
+	});
+	if(extensionHookedTransmit)
+		return transmitStatusFromExtension;
+
 	std::vector<uint8_t> packet;
 	if(!com->encoder->encode(packet, message))
 		return false;
@@ -338,10 +352,18 @@ void Device::addExtension(std::shared_ptr<DeviceExtension>&& extension) {
 	extensions.push_back(extension);
 }
 
-void Device::forEachExtension(std::function<void(const std::shared_ptr<DeviceExtension>&)> fn) {
+void Device::forEachExtension(std::function<bool(const std::shared_ptr<DeviceExtension>&)> fn) {
-	std::lock_guard<std::mutex> lk(extensionsLock);
+	std::vector<std::shared_ptr<DeviceExtension>> extensionsCopy;
-	for(const auto& ext : extensions)
+
-		fn(ext);
+	{
+		std::lock_guard<std::mutex> lk(extensionsLock);
+		extensionsCopy = extensions;
+	}
+
+	for(const auto& ext : extensionsCopy) {
+		if(!fn(ext))
+			break;
+	}
 }
 
 void Device::handleInternalMessage(std::shared_ptr<Message> message) {
@@ -354,6 +376,7 @@ void Device::handleInternalMessage(std::shared_ptr<Message> message) {
 	}
 	forEachExtension([&](const std::shared_ptr<DeviceExtension>& ext) {
 		ext->handleMessage(message);
+		return true; // false breaks out early
 	});
 }
 

device/extensions/flexray/controller.cpp

@@ -3,28 +3,452 @@
 
 using namespace icsneo;
 
-std::shared_ptr<FlexRayControlMessage> FlexRay::Controller::getStatus() const {
-	std::lock_guard<std::mutex> lk(statusLock);
-	return status;
-}
-
 void FlexRay::Controller::_setStatus(std::shared_ptr<FlexRayControlMessage> msg) {
 	std::lock_guard<std::mutex> lk(statusLock);
 	status = msg;
 }
 
-void FlexRay::Controller::getReady() {
+std::shared_ptr<FlexRayControlMessage> FlexRay::Controller::getStatus() const {
-	
+	std::lock_guard<std::mutex> lk(statusLock);
+	return status;
 }
 
-void FlexRay::Controller::start() {
+std::pair<const FlexRay::Cluster::Configuration&, const FlexRay::Controller::Configuration&> FlexRay::Controller::getConfiguration() const {
-	if(true) // TODO something
+	return { clusterConfig, controllerConfig };
-		getReady();
+}
-	if(wakeupBeforeStart)
+
-		setCurrentPOCCommand(FlexRay::POCCommand::Wakeup);
+void FlexRay::Controller::setConfiguration(Cluster::Configuration clConfig, Controller::Configuration coConfig) {
-	if(allowColdstart)
+	configDirty = true;
-		setCurrentPOCCommand(FlexRay::POCCommand::AllowColdstart);
+	clusterConfig = clConfig;
-	setCurrentPOCCommand(FlexRay::POCCommand::Run);
+	controllerConfig = coConfig;
+}
+
+void FlexRay::Controller::addMessageBuffer(MessageBuffer buffer) {
+	configDirty = true;
+	messageBuffers.emplace_back(std::move(buffer));
+}
+
+void FlexRay::Controller::clearMessageBuffers() {
+	configDirty = true;
+	messageBuffers.clear();
+}
+
+bool FlexRay::Controller::wakeup(std::chrono::milliseconds timeout) {
+	return setCurrentPOCCommand(FlexRay::POCCommand::Wakeup, true, timeout);
+}
+
+bool FlexRay::Controller::configure(std::chrono::milliseconds timeout) {
+	const auto initialTimeout = timeout;
+	const auto functionBegin = std::chrono::steady_clock::now();
+	const auto updateTimeout = [&initialTimeout, &functionBegin, &timeout]() {
+		timeout = std::chrono::duration_cast<std::chrono::milliseconds>(initialTimeout - (std::chrono::steady_clock::now() - functionBegin));
+	};
+
+	auto statusPair = getCurrentPOCStatus(timeout);
+	const auto& status = statusPair.second;
+	if(!statusPair.first)
+		return false;
+	updateTimeout();
+
+	if(status != POCStatus::Config) {
+		if(!enterConfig(timeout))
+			return false;
+		updateTimeout();
+	}
+
+	if(!setCurrentPOCCommand(POCCommand::ClearRAMs, true, timeout))
+		return false;
+	const auto start = std::chrono::steady_clock::now();
+	bool carbusy = isClearAllRAMBusy();
+	while(carbusy && (std::chrono::steady_clock::now() - start) < timeout) {
+		carbusy = isClearAllRAMBusy();
+	}
+	if(carbusy) // timeout
+		return false;
+	updateTimeout();
+
+	std::vector<std::pair<ERAYRegister, uint32_t>> registerWrites;
+	registerWrites.reserve(18);
+
+	registerWrites.push_back({ ERAYRegister::SUCC1,
+		((controllerConfig.KeySlotUsedForStartup & 0x1) << 8) | // TXST
+		((controllerConfig.KeySlotUsedForSync & 0x1) << 9) | // TXSY
+		((clusterConfig.ColdStartAttempts & 0x1f) << 11) | // CSA
+		((controllerConfig.AllowPassiveToActiveCyclePairs & 0x1f) << 16) | // PTA
+		((controllerConfig.WakeupOnChannelB & 0x1) << 21) | // WUCS
+		((controllerConfig.KeySlotOnlyEnabled & 0x1) << 22) | // TSM
+		((controllerConfig.AllowHaltDueToClock & 0x1) << 23) | // HCSE
+		((controllerConfig.MTSOnA & 0x1) << 24) | // MTSA
+		((controllerConfig.MTSOnB & 0x1) << 25) | // MTSB
+		((controllerConfig.ChannelA & 0x1) << 26) | // CCHA
+		((controllerConfig.ChannelB & 0x1) << 27) // CCHB
+	});
+
+	registerWrites.push_back({ ERAYRegister::SUCC2,
+		((controllerConfig.ListenTimeout & 0x1fffff)) |
+		(clusterConfig.ListenNoiseMacroticks << 24)
+	});
+
+	registerWrites.push_back({ ERAYRegister::SUCC3,
+		(clusterConfig.MaxWithoutClockCorrectionPassive & 0xF) |
+		((clusterConfig.MaxWithoutClockCorrectionFatal) << 4)
+	});
+
+	registerWrites.push_back({ ERAYRegister::NEMC,
+		clusterConfig.NetworkManagementVectorLengthBytes
+	});
+
+	registerWrites.push_back({ ERAYRegister::PRTC1,
+		(clusterConfig.TransmissionStartSequenceDurationBits & 0xF) |
+		((clusterConfig.CASRxLowMax & 0x3F) << 4) |
+		((clusterConfig.StrobePointPosition & 0x3) << 12) |
+		((clusterConfig.Speed & 0x3) << 14) |
+		((clusterConfig.WakeupRxWindowBits & 0x1F) << 16) |
+		((controllerConfig.WakeupPattern) << 26)
+	});
+
+	registerWrites.push_back({ ERAYRegister::PRTC2,
+		(clusterConfig.WakeupRxIdleBits) |
+		((clusterConfig.WakeupRxLowBits) << 8) |
+		((clusterConfig.WakeupTxIdleBits) << 16) |
+		((clusterConfig.WakeupTxActiveBits) << 24)
+	});
+
+	registerWrites.push_back({ ERAYRegister::MHDC,
+		(clusterConfig.PayloadLengthOfStaticSlotInWords) |
+		((controllerConfig.LatestTxMinislot) << 16)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC1,
+		controllerConfig.MicroPerCycle
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC2,
+		(clusterConfig.SyncFrameIDCountMax << 16) |
+		clusterConfig.MacroticksPerCycle
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC3,
+		(controllerConfig.MicroInitialOffsetA) |
+		((controllerConfig.MicroInitialOffsetB) << 8) |
+		((controllerConfig.MacroInitialOffsetA) << 16) |
+		((controllerConfig.MacroInitialOffsetB) << 24)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC4,
+		((clusterConfig.MacroticksPerCycle - clusterConfig.NetworkIdleTimeMacroticks - 1) & 0xFFFF) |
+		((clusterConfig.OffsetCorrectionStartMacroticks - 1) << 16)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC5,
+		controllerConfig.DelayCompensationAMicroticks |
+		(controllerConfig.DelayCompensationBMicroticks << 8) |
+		(controllerConfig.ClusterDriftDamping << 16) |
+		(controllerConfig.DecodingCorrectionMicroticks << 24)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC6,
+		controllerConfig.AcceptStartupRangeMicroticks |
+		(controllerConfig.RateCorrectionOutMicroticks << 16)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC7,
+		clusterConfig.NumberOfStaticSlots |
+		(clusterConfig.StaticSlotMacroticks << 16)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC8,
+		clusterConfig.MinislotDurationMacroticks |
+		(clusterConfig.NumberOfMinislots << 16)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC9,
+		clusterConfig.ActionPointOffset |
+		(clusterConfig.MinislotActionPointOffsetMacroticks << 8) |
+		(clusterConfig.DynamicSlotIdlePhaseMinislots << 16)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC10,
+		controllerConfig.OffsetCorrectionOutMicroticks |
+		(controllerConfig.RateCorrectionOutMicroticks << 16)
+	});
+
+	registerWrites.push_back({ ERAYRegister::GTUC11,
+		controllerConfig.ExternOffsetCorrectionControl |
+		(controllerConfig.ExternRateCorrectionControl << 8) |
+		(controllerConfig.ExternOffsetCorrectionMicroticks << 16) |
+		(controllerConfig.ExternRateCorrectionMicroticks << 24)
+	});
+
+	std::vector<MessageBuffer> staticTx, dynamicTx;
+
+	// Add key slot messages
+	MessageBuffer first;
+	bool firstIsInMessageBuffers = false;
+	bool firstUsed = false;
+
+	MessageBuffer second;
+	bool secondIsInMessageBuffers = false;
+	bool secondUsed = false;
+
+	if(controllerConfig.KeySlotUsedForSync || controllerConfig.KeySlotOnlyEnabled) {
+		first.isStartup = controllerConfig.KeySlotUsedForStartup;
+		first.isSync = controllerConfig.KeySlotUsedForSync;
+		first.isTransmit = true;
+		first.channelA = true;
+		first.channelB = !controllerConfig.TwoKeySlotMode && controllerConfig.ChannelB;
+		first.frameID = controllerConfig.KeySlotID;
+		first.frameLengthBytes = clusterConfig.PayloadLengthOfStaticSlotInWords * 2;
+		first.baseCycle = 0;
+		first.cycleRepetition = 1;
+		first.continuousMode = 1;
+		staticTx.emplace_back(first);
+		firstUsed = true;
+
+		if(controllerConfig.TwoKeySlotMode) {
+			second.isStartup = controllerConfig.KeySlotUsedForStartup;
+			second.isSync = controllerConfig.KeySlotUsedForSync;
+			second.isTransmit = true;
+			second.channelB =true;
+			second.frameID = controllerConfig.SecondKeySlotID;
+			second.frameLengthBytes = clusterConfig.PayloadLengthOfStaticSlotInWords * 2;
+			second.baseCycle = 0;
+			second.cycleRepetition = 1;
+			second.continuousMode = 0;
+			staticTx.emplace_back(second);
+			secondUsed = true;
+		}
+	}
+
+	for(const auto& buf : messageBuffers) {
+		if(!buf.isTransmit)
+			continue; // Only transmit frames need to be written to the controller
+
+		if(!buf.isDynamic && ((controllerConfig.KeySlotUsedForSync && buf.isSync) || (controllerConfig.KeySlotUsedForStartup && buf.isStartup))) {
+			if(staticTx[0].frameID == buf.frameID) {
+				staticTx[0].frameLengthBytes = buf.frameLengthBytes;
+				staticTx[0].baseCycle = buf.baseCycle;
+				staticTx[0].cycleRepetition = buf.cycleRepetition;
+				staticTx[0].continuousMode = buf.continuousMode;
+				firstIsInMessageBuffers = true;
+				continue;
+			}
+
+			if(controllerConfig.TwoKeySlotMode && staticTx[1].frameID == buf.frameID) {
+				staticTx[1].frameLengthBytes = buf.frameLengthBytes;
+				staticTx[1].baseCycle = buf.baseCycle;
+				staticTx[1].cycleRepetition = buf.cycleRepetition;
+				staticTx[1].continuousMode = buf.continuousMode;
+				secondIsInMessageBuffers = true;
+				continue;
+			}
+		}
+
+		if(buf.isDynamic)
+			dynamicTx.push_back(buf);
+		else
+			staticTx.push_back(buf);
+	}
+
+	// If the user is using the default coldstart messages, they need to be added to the list for transmit
+	if(firstUsed && !firstIsInMessageBuffers)
+		messageBuffers.push_back(first);
+	if(secondUsed && !secondIsInMessageBuffers)
+		messageBuffers.push_back(second);
+
+	int64_t totalBuffers = staticTx.size() + dynamicTx.size();
+	if(totalBuffers > 128) // TODO warn
+		totalBuffers = 128;
+	totalBuffers -= 1;
+
+	registerWrites.push_back({ ERAYRegister::MRC,
+		// FDB[7:0] set to 0, No group of message buffers exclusively for the static segment configured
+		// FFB[7:0] set to 0x80, No message buffer assigned to the FIFO
+		(0x80 << 8) |
+		(uint8_t(totalBuffers) << 16) |
+		(controllerConfig.TwoKeySlotMode << 2)
+	});
+
+	for(const auto& regpair : registerWrites) {
+		if(!writeRegister(regpair.first, regpair.second, true, timeout))
+			return false;
+		updateTimeout();
+	}
+
+	uint16_t dataPointer = (totalBuffers + 1) * 4;
+	for(auto i = 0; i <= totalBuffers; i++) {
+		auto& buf = (i < staticTx.size() ? staticTx[i] : dynamicTx[i - staticTx.size()]);
+
+		if(buf.frameID == 0)
+			buf.frameID = i | (1 << 10);
+
+		uint32_t hs1 = (
+			(buf.frameID) |
+			(CalculateCycleFilter(buf.baseCycle, buf.cycleRepetition) << 16) |
+			((buf.channelA & 0x1) << 24) |
+			((buf.channelB & 0x1) << 25) |
+			((buf.isTransmit & 0x1) << 26) | // CFG
+			((buf.isNMFrame & 0x1) << 27) | // PPIT
+			((!buf.continuousMode & 0x1) << 28) | // TXM
+			((0 & 0x1) << 29) // MBI, disabled for now but we might want confirmations in the future
+		);
+
+		uint32_t hs2 = (
+			CalculateHCRC(buf) |
+			(((buf.frameLengthBytes + 1) / 2) << 16)
+		);
+
+		uint32_t hs3 = dataPointer;
+		buf._dataPointer = dataPointer;
+		buf._id = i;
+		dataPointer += buf.frameLengthBytes / 4;
+		dataPointer += dataPointer % 4; // must be a 4 byte boundary
+
+		if(!writeRegister(ERAYRegister::WRHS1, hs1, true, timeout))
+			return false;
+		updateTimeout();
+
+		if(!writeRegister(ERAYRegister::WRHS2, hs2, true, timeout))
+			return false;
+		updateTimeout();
+
+		if(!writeRegister(ERAYRegister::WRHS3, hs3, true, timeout))
+			return false;
+		updateTimeout();
+
+		if(!writeRegister(ERAYRegister::IBCM, 1, true, timeout))
+			return false;
+		const auto ibcmstart = std::chrono::steady_clock::now();
+		bool ibcmbusy = isInputBufferHostBusy();
+		while(ibcmbusy && (std::chrono::steady_clock::now() - ibcmstart) < timeout) {
+			ibcmbusy = isInputBufferHostBusy();
+		}
+		if(ibcmbusy) // timeout
+			return false;
+		updateTimeout();
+
+		if(!writeRegister(ERAYRegister::IBCR, i, true, timeout))
+			return false;
+		updateTimeout();
+	}
+
+	configDirty = false;
+	return true;
+}
+
+bool FlexRay::Controller::getReady(std::chrono::milliseconds timeout) {
+	const auto initialTimeout = timeout;
+	const auto functionBegin = std::chrono::steady_clock::now();
+	const auto updateTimeout = [&initialTimeout, &functionBegin, &timeout]() {
+		timeout = std::chrono::duration_cast<std::chrono::milliseconds>(initialTimeout - (std::chrono::steady_clock::now() - functionBegin));
+	};
+
+	// Initial sanity check that we have communication with the controller
+	auto endian = readRegister(ERAYRegister::ENDN, timeout);
+	if (!endian.first || endian.second != 0x87654321)
+		return false;
+	updateTimeout();
+
+	auto statusPair = getCurrentPOCStatus(timeout);
+	const auto& status = statusPair.second;
+	if(!statusPair.first)
+		return false;
+	updateTimeout();
+
+	if(status == POCStatus::Ready && !configDirty)
+		return true; // Already in the desired state
+
+	if(status != POCStatus::Config) {
+		// Must enter config before continuing
+		if(!enterConfig(timeout))
+			return false;
+		updateTimeout();
+
+		// Reconfigure if necessary
+		if(configDirty && !configure(timeout))
+			return false;
+		updateTimeout();
+	}
+
+	// Enter the READY state
+	if(!lockConfiguration(timeout))
+		return false;
+	updateTimeout();
+
+	// Signal that we'd like to coldstart, if necessary
+	if(allowColdstart && !setCurrentPOCCommand(FlexRay::POCCommand::AllowColdstart, true, timeout))
+		return false;
+	return true;
+}
+
+bool FlexRay::Controller::start(std::chrono::milliseconds timeout) {
+	const auto initialTimeout = timeout;
+	const auto functionBegin = std::chrono::steady_clock::now();
+	const auto updateTimeout = [&initialTimeout, &functionBegin, &timeout]() {
+		timeout = std::chrono::duration_cast<std::chrono::milliseconds>(initialTimeout - (std::chrono::steady_clock::now() - functionBegin));
+	};
+
+	// First make sure we're ready to start (configured/ready state)
+	if(!getReady(timeout))
+		return false;
+	updateTimeout();
+
+	// Wakeup the network if necessary
+	if(wakeupBeforeStart && !wakeup(timeout))
+		return false;
+	updateTimeout();
+
+	// And finally run
+	if(!setCurrentPOCCommand(FlexRay::POCCommand::Run, false, timeout))
+		return false;
+	return true;
+}
+
+bool FlexRay::Controller::transmit(const std::shared_ptr<FlexRayMessage>& frmsg) {
+	bool success = false;
+
+	for(const auto& buf : messageBuffers) {
+		if(!buf.isTransmit)
+			continue;
+
+		if(frmsg->slotid != buf.frameID)
+			continue;
+
+		if(CalculateCycleFilter(frmsg->cycle, frmsg->cycleRepetition) != CalculateCycleFilter(buf.baseCycle, buf.cycleRepetition))
+			continue;
+
+		FlexRay::Channel bufChannel = FlexRay::Channel::None;
+		if(buf.channelA && buf.channelB)
+			bufChannel = FlexRay::Channel::AB;
+		else if(buf.channelA)
+			bufChannel = FlexRay::Channel::A;
+		else if(buf.channelB)
+			bufChannel = FlexRay::Channel::B;
+		
+		if(frmsg->channel != bufChannel)
+			continue;
+
+		// This is a message buffer we want to fill
+		if(!device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildWriteMessageBufferArgs(index, buf._id, frmsg->data, buf.frameLengthBytes)))
+			continue;
+
+		success = true;
+	}
+
+	return success;
+}
+
+bool FlexRay::Controller::halt(std::chrono::milliseconds timeout) {
+	return setCurrentPOCCommand(POCCommand::Halt, true, timeout);
+}
+
+bool FlexRay::Controller::freeze(std::chrono::milliseconds timeout) {
+	return setCurrentPOCCommand(POCCommand::Freeze, true, timeout);
+}
+
+bool FlexRay::Controller::triggerMTS(std::chrono::milliseconds timeout) {
+	// triggerMTS will do nothing unless either MTSOnA or MTSOnB (or both) are set at configure time
+	return setCurrentPOCCommand(POCCommand::SendMTS, true, timeout);
 }
 
 std::pair<bool, FlexRay::POCCommand> FlexRay::Controller::getCurrentPOCCommand(std::chrono::milliseconds timeout) const {
@@ -34,26 +458,136 @@ std::pair<bool, FlexRay::POCCommand> FlexRay::Controller::getCurrentPOCCommand(s
 
 bool FlexRay::Controller::setCurrentPOCCommand(FlexRay::POCCommand cmd, bool checkForSuccess, std::chrono::milliseconds timeout) {
 	const auto beforeWrite = std::chrono::steady_clock::now();
-
+	if(!writeRegister(ERAYRegister::SUCC1, uint32_t(cmd), 0xF, true, timeout))
-	if(!writeRegister(ERAYRegister::SUCC1, uint32_t(cmd), true, timeout))
 		return false;
 	if(!checkForSuccess)
 		return true;
 	
 	const auto writeDuration = std::chrono::steady_clock::now() - beforeWrite;
-	timeout = std::chrono::duration_cast<std::chrono::milliseconds>(writeDuration - timeout);
+	timeout = std::chrono::duration_cast<std::chrono::milliseconds>(timeout - writeDuration);
-	if(timeout.count() <= 0)
-		return false; // Out of time!
 
 	return wasCommandSuccessful(timeout);
 }
 
 bool FlexRay::Controller::wasCommandSuccessful(std::chrono::milliseconds timeout) const {
+	const auto start = std::chrono::steady_clock::now();
+	bool pocBusy = isPOCBusy();
+	while(pocBusy && (std::chrono::steady_clock::now() - start) < timeout) {
+		pocBusy = isPOCBusy();
+	}
+	if(pocBusy) // timeout
+		return false;
+	timeout = std::chrono::duration_cast<std::chrono::milliseconds>(timeout - (std::chrono::steady_clock::now() - start));
+
 	const auto val = getCurrentPOCCommand(timeout);
 	return val.first && val.second != FlexRay::POCCommand::CommandNotAccepted;
 }
 
+std::pair<bool, FlexRay::POCStatus> FlexRay::Controller::getCurrentPOCStatus(std::chrono::milliseconds timeout) const {
+	auto regpair = readRegister(ERAYRegister::CCSV, timeout);
+	return { regpair.first, FlexRay::POCStatus(regpair.second & 0x3F) };
+}
+
+bool FlexRay::Controller::lockConfiguration(std::chrono::milliseconds timeout) {
+	// This is not anything super special, just the way to get the ERAY out of POC:config
+	// See the ERAY Users Manaual section 4.3.1
+
+	auto beforeWrite = std::chrono::steady_clock::now();
+	if(!writeRegister(ERAYRegister::LCK, 0xCE, true, timeout))
+		return false;
+	timeout = std::chrono::duration_cast<std::chrono::milliseconds>(timeout - (std::chrono::steady_clock::now() - beforeWrite));
+	if(timeout.count() <= 0)
+		return false; // Out of time!
+
+	beforeWrite = std::chrono::steady_clock::now();
+	if(!writeRegister(ERAYRegister::LCK, 0x31, true, timeout))
+		return false;
+	timeout = std::chrono::duration_cast<std::chrono::milliseconds>(timeout - (std::chrono::steady_clock::now() - beforeWrite));
+
+	return setCurrentPOCCommand(POCCommand::Ready, true, timeout);
+}
+
+bool FlexRay::Controller::enterConfig(std::chrono::milliseconds timeout) {
+	const auto initialTimeout = timeout;
+	const auto functionBegin = std::chrono::steady_clock::now();
+	const auto updateTimeout = [&initialTimeout, &functionBegin, &timeout]() {
+		timeout = std::chrono::duration_cast<std::chrono::milliseconds>(initialTimeout - (std::chrono::steady_clock::now() - functionBegin));
+	};
+
+	auto statusPair = getCurrentPOCStatus(timeout);
+	const auto& status = statusPair.second;
+	if(!statusPair.first)
+		return false;
+	updateTimeout();
+
+	if(status != FlexRay::POCStatus::Ready &&
+		status != FlexRay::POCStatus::Config &&
+		status != FlexRay::POCStatus::DefaultConfig &&
+		status != FlexRay::POCStatus::Halt) {
+		if(!setCurrentPOCCommand(FlexRay::POCCommand::Freeze, true, timeout))
+			return false;
+		updateTimeout();
+	}
+
+	// If we're halted, we first go into DEFAULT_CONFIG before entering CONFIG
+	// Unintuitively, this enters DEFAULT_CONFIG
+	if(!setCurrentPOCCommand(FlexRay::POCCommand::Config, true, timeout))
+		return false;
+	updateTimeout();
+
+	// Now this enters CONFIG
+	return setCurrentPOCCommand(FlexRay::POCCommand::Config, true, timeout);
+}
+
+uint16_t FlexRay::Controller::CalculateHCRC(const MessageBuffer& buf) {
+	uint16_t ret = 0x1A;
+
+	auto addBit = [&ret](uint8_t bit) {
+		bit = bit ? 1 : 0;
+
+		int crcNxt; //CRCNXT = NXTBIT EXOR CRC_RG(14);
+		if (ret & (1<<10))
+			crcNxt = bit ^ 1;
+		else
+			crcNxt = bit ^ 0;
+		crcNxt &= 0x01;
+
+		// CRC_RG(14:1) = CRC_RG(13:0); // shift left by
+		ret <<= 1;
+		ret &= 0x7FE; // clear first bit
+
+		if (crcNxt) //CRC_RG(14:0) = CRC_RG(14:0) EXOR (4599hex);
+			ret ^= 0x385;
+	};
+
+	addBit(buf.isStartup);
+	addBit(buf.isSync);
+	for(auto i = 0; i < 11; i++)
+		addBit(buf.frameID & (1 << (10 - i)));
+	for(auto i = 0; i < 7; i++)
+		addBit(((buf.frameLengthBytes + 1) / 2) & (1 << (6 - i)));
+
+	return ret;
+}
+
+uint16_t FlexRay::Controller::CalculateCycleFilter(uint8_t baseCycle, uint8_t cycleRepetition) {
+	uint8_t cycleRepCode = 0;
+	switch(cycleRepetition) {
+		case 1: cycleRepCode = 0b1; break;
+		case 2: cycleRepCode = 0b10; break;
+		case 4: cycleRepCode = 0b100; break;
+		case 8: cycleRepCode = 0b1000; break;
+		case 16: cycleRepCode = 0b10000; break;
+		case 32: cycleRepCode = 0b100000; break;
+		case 64: cycleRepCode = 0b1000000; break;
+	}
+	return (cycleRepCode | baseCycle);
+}
+
 std::pair<bool, uint32_t> FlexRay::Controller::readRegister(ERAYRegister reg, std::chrono::milliseconds timeout) const {
+	if(timeout.count() <= 0)
+		return {false, 0}; // Out of time!
+
 	std::lock_guard<std::mutex> lk(readRegisterLock);
 	device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildReadCCRegsArgs(index, uint16_t(reg)));
 	std::shared_ptr<FlexRayControlMessage> resp;
@@ -65,7 +599,7 @@ std::pair<bool, uint32_t> FlexRay::Controller::readRegister(ERAYRegister reg, st
 				resp = frmsg;
 		}
 	}
-	if(resp)
+	if(resp && !resp->registers.empty())
 		return {true, resp->registers[0]};
 	else
 		return {false, 0};
@@ -83,15 +617,23 @@ bool FlexRay::Controller::writeRegister(
 	bool waitForPOCReady,
 	std::chrono::milliseconds timeout) {
 
+	if(waitForPOCReady) {
+		const auto start = std::chrono::steady_clock::now();
+		bool pocBusy = isPOCBusy();
+		while(pocBusy && (std::chrono::steady_clock::now() - start) < timeout) {
+			pocBusy = isPOCBusy();
+		}
+		if(pocBusy) // timeout
+			return false;
+	}
+
 	if(mask != 0xffffffff) {
 		const auto beforeRead = std::chrono::steady_clock::now();
 		auto pair = readRegister(reg, timeout);
 		if(!pair.first)
 			return false; // Couldn't read, so we don't want to try to write anything
 		auto readDuration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - beforeRead);
-		timeout = readDuration - timeout;
+		timeout -= readDuration;
-		if(timeout.count() <= 0)
-			return false; // Out of time!
 		pair.second &= ~mask;
 		pair.second |= value & mask;
 		value = pair.second;
@@ -104,6 +646,8 @@ bool FlexRay::Controller::writeRegister(
 	uint32_t value,
 	bool waitForPOCReady,
 	std::chrono::milliseconds timeout) {
+	if(timeout.count() <= 0)
+		return false; // Out of time!
 
 	if(waitForPOCReady) {
 		const auto start = std::chrono::steady_clock::now();
@@ -115,6 +659,7 @@ bool FlexRay::Controller::writeRegister(
 			return false;
 	}
 
-	device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildWriteCCRegArgs(index, uint16_t(reg), value));
+	if(!device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildWriteCCRegArgs(index, uint16_t(reg), value)))
-	return true; // Does the device send anything back to tell us this actually happened?
+		return false;
+	return true; // The device does not confirm the the command, if it did we'd put that here
 }

device/extensions/flexray/extension.cpp

@@ -1,11 +1,28 @@
 #include "icsneo/device/extensions/flexray/extension.h"
 #include "icsneo/device/device.h"
+#include "icsneo/communication/message/flexray/flexraymessage.h"
 
 using namespace icsneo;
 
-FlexRay::Extension::Extension(Device& device, uint8_t controllerCount) : DeviceExtension(device) {
+FlexRay::Extension::Extension(Device& device, const std::vector<Network>& controllerNetworks) : DeviceExtension(device) {
-	for(uint8_t i = 0; i < controllerCount; i++)
+	for(uint8_t i = 0; i < controllerNetworks.size(); i++)
-		controllers.emplace_back(std::make_shared<FlexRay::Controller>(device, i));
+		controllers.emplace_back(std::make_shared<FlexRay::Controller>(device, i, controllerNetworks[i]));
+}
+
+void FlexRay::Extension::onGoOnline() {
+	for(auto& controller : controllers) {
+		if(controller->getStartWhenGoingOnline())
+			controller->getReady();
+	}
+	for(auto& controller : controllers) {
+		if(controller->getStartWhenGoingOnline())
+			controller->start();
+	}
+}
+
+void FlexRay::Extension::onGoOffline() {
+	for(auto& controller : controllers)
+		controller->halt();
 }
 
 void FlexRay::Extension::handleMessage(const std::shared_ptr<Message>& message) {
@@ -28,4 +45,23 @@ void FlexRay::Extension::handleMessage(const std::shared_ptr<Message>& message)
 		default:
 			break;
 	}
+}
+
+bool FlexRay::Extension::transmitHook(const std::shared_ptr<Message>& message, bool& success) {
+	if(!message || message->network.getType() != Network::Type::FlexRay)
+		return true; // Don't hook non-FlexRay messages
+
+	success = false;
+
+	std::shared_ptr<FlexRayMessage> frmsg = std::dynamic_pointer_cast<FlexRayMessage>(message);
+	if(!frmsg)
+		return false;
+
+	for(auto& controller : controllers) {
+		if(controller->getNetwork() != message->network)
+			continue;
+		success |= controller->transmit(frmsg);
+	}
+
+	return false;
 }

include/icsneo/communication/command.h

@@ -5,6 +5,7 @@ namespace icsneo {
 
 enum class Command : uint8_t {
 	EnableNetworkCommunication = 0x07,
+	EnableNetworkCommunicationEx = 0x08,
 	RequestSerialNumber = 0xA1,
 	SetSettings = 0xA4, // Previously known as RED_CMD_SET_BAUD_REQ, follow up with SaveSettings to write to EEPROM
 	//GetSettings = 0xA5, // Previously known as RED_CMD_READ_BAUD_REQ, now unused

include/icsneo/communication/message/filter/messagefilter.h

@@ -11,7 +11,7 @@ class MessageFilter {
 public:
 	MessageFilter() {}
 	MessageFilter(Network::Type type) : type(type) {}
-	MessageFilter(Network::NetID netid) : netid(netid) {}
+	MessageFilter(Network::NetID netid) : type(Network::GetTypeOfNetID(netid)), netid(netid) {}
 	virtual ~MessageFilter() {}
 	// When getting "all" types of messages, include the ones marked as "internal only"
 	bool includeInternalInAny = false;

include/icsneo/communication/message/flexray/control/flexraycontrolmessage.h

@@ -12,11 +12,13 @@ namespace icsneo {
 // Internal message which gives us the state of the FlexRay Controllers
 class FlexRayControlMessage : public Message {
 public:
-	static std::vector<uint8_t> BuildBaseControlArgs(uint8_t controller, FlexRay::Opcode op, std::initializer_list<uint8_t> args);
+	static std::vector<uint8_t> BuildBaseControlArgs(uint8_t controller, FlexRay::Opcode op, const std::vector<uint8_t>& args);
 	static std::vector<uint8_t> BuildReadCCRegsArgs(uint8_t controller, uint16_t startAddress, uint8_t numRegisters = 1);
 	static std::vector<uint8_t> BuildWriteCCRegArgs(uint8_t controller, uint16_t address, uint32_t value);
 	static std::vector<uint8_t> BuildAddConfiguredTxMessageArgs(
 		uint8_t controller, uint16_t descriptionId, uint16_t slotId, uint8_t baseCycle, uint8_t cycleReps, FlexRay::Channel channel);
+	static std::vector<uint8_t> BuildWriteMessageBufferArgs(
+		uint8_t controller, uint16_t bufferId, const std::vector<uint8_t>& data, uint16_t desiredSize);
 
 	FlexRayControlMessage(const Packet& packet);
 	virtual ~FlexRayControlMessage() = default;

include/icsneo/communication/message/flexray/flexraymessage.h

@@ -25,7 +25,8 @@ public:
 	bool sync = false;
 	bool startup = false;
 	bool dynamic = false;
-	uint8_t cycle = 0;
+	uint8_t cycle = 0; // baseCycle when transmitting
+	uint8_t cycleRepetition = 0; // only for transmit
 };
 
 }

include/icsneo/communication/message/main51message.h

@@ -2,14 +2,15 @@
 #define __MAIN51MESSAGE_H_
 
 #include "icsneo/communication/message/message.h"
-#include "icsneo/communication/communication.h"
+#include "icsneo/communication/command.h"
 
 namespace icsneo {
 
 class Main51Message : public Message {
 public:
 	virtual ~Main51Message() = default;
-	Command command;
+	Command command = Command(0);
+	bool forceShortFormat = false; // Necessary for EnableNetworkCom and EnableNetworkComEx
 };
 
 }

include/icsneo/communication/network.h

@@ -433,6 +433,7 @@ public:
 		return os;
 	}
 	friend bool operator==(const Network& net1, const Network& net2) { return net1.getNetID() == net2.getNetID(); }
+	friend bool operator!=(const Network& net1, const Network& net2) { return !(net1 == net2); }
 
 private:
 	NetID value; // Always use setValue so that value and type stay in sync

include/icsneo/device/device.h

@@ -86,7 +86,7 @@ public:
 	virtual size_t getNetworkCountByType(Network::Type) const;
 	virtual Network getNetworkByNumber(Network::Type, size_t) const;
 
-	virtual std::shared_ptr<FlexRay::Controller> getFlexRayControllerByNetwork(const Network&) const { return nullptr; }
+	virtual std::vector<std::shared_ptr<FlexRay::Controller>> getFlexRayControllers() const { return {}; }
 
 	const device_eventhandler_t& getEventHandler() const { return report; }
 
@@ -186,7 +186,7 @@ private:
 
 	mutable std::mutex extensionsLock;
 	std::vector<std::shared_ptr<DeviceExtension>> extensions;
-	void forEachExtension(std::function<void(const std::shared_ptr<DeviceExtension>&)> fn);
+	void forEachExtension(std::function<bool(const std::shared_ptr<DeviceExtension>&)> fn);
 
 	std::vector<Network> supportedTXNetworks;
 	std::vector<Network> supportedRXNetworks;

include/icsneo/device/extensions/deviceextension.h

@@ -14,8 +14,17 @@ public:
 	DeviceExtension(Device& device) : device(device) {}
 	virtual ~DeviceExtension() = default;
 	virtual const char* getName() const = 0;
+
+	virtual void onDeviceOpen() {}
+	virtual void onGoOnline() {}
+	virtual void onGoOffline() {}
+	virtual void onDeviceClose() {}
+
 	virtual void handleMessage(const std::shared_ptr<Message>&) {}
 
+	// Return true to continue transmitting, success should be written to if false is returned
+	virtual bool transmitHook(const std::shared_ptr<Message>& message, bool& success) { return true; }
+
 protected:
 	Device& device;
 };

include/icsneo/device/extensions/flexray/cluster.h

@@ -0,0 +1,110 @@
+#ifndef __FLEXRAYCLUSTER_H_
+#define __FLEXRAYCLUSTER_H_
+
+#ifdef __cplusplus
+
+#include <cstdint>
+#include "icsneo/communication/message/flexray/control/flexraycontrolmessage.h"
+#include "icsneo/device/extensions/flexray/erayregister.h"
+#include "icsneo/device/extensions/flexray/poccommand.h"
+
+namespace icsneo {
+
+namespace FlexRay {
+
+namespace Cluster {
+
+#endif // __cplusplus
+
+typedef enum {
+	FLEXRAY_BAUDRATE_10M = 0,
+	FLEXRAY_BAUDRATE_5M = 1, // 5Mbit currently not supported in the monitor
+	FLEXRAY_BAUDRATE_2M5 = 2, // 2.5Mbit currently not supported in the monitor
+	FLEXRAY_BAUDRATE_2M5_ALT = 3 // Per the ERAY spec, 0b11 also means 2.5MBits
+} neoflexray_speed_t;
+
+typedef enum {
+	FLEXRAY_SPP_5 = 0, // FlexRay 2.1 requires this value to be 0
+	FLEXRAY_SPP_4 = 1,
+	FLEXRAY_SPP_6 = 2,
+	FLEXRAY_SPP_5_ALT = 3 // Per the ERAY spec, 0b11 also means sample point 5
+} neoflexray_spp_t;
+
+#ifdef __cplusplus
+
+#define INIT(x) = x
+
+#define neoflexray_cluster_config_t icsneo::FlexRay::Cluster::Configuration
+struct Configuration {
+
+#else // __cplusplus
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define INIT(x)
+
+typedef struct {
+
+#endif // __cplusplus
+
+	neoflexray_speed_t Speed INIT(FLEXRAY_BAUDRATE_10M /* = 0 */); // gdSampleClockPeriod, pSamplesPerMicrotick, Baud Rate Prescaler
+	neoflexray_spp_t StrobePointPosition INIT(FLEXRAY_SPP_5 /* = 0 */);
+
+	uint8_t ActionPointOffset INIT(0);
+	uint32_t BitTimeNS INIT(0);
+	uint8_t CASRxLowMax INIT(0);
+	uint8_t ColdStartAttempts INIT(0);
+	uint32_t CycleCountMax INIT(0);
+	double CycleDurationSec INIT(0);
+	bool DetectNITError INIT(false);
+	uint8_t DynamicSlotIdlePhaseMinislots INIT(0); // In Minislot increments [0..2]
+	uint32_t IgnoreAfterTx INIT(0);
+	uint8_t ListenNoiseMacroticks INIT(0);
+	uint16_t MacroticksPerCycle INIT(0);
+	double MacrotickDurationSec INIT(0);
+	uint8_t MaxWithoutClockCorrectionFatal INIT(0);
+	uint8_t MaxWithoutClockCorrectionPassive INIT(0);
+	uint8_t MinislotActionPointOffsetMacroticks INIT(0);
+	uint32_t MinislotDurationMacroticks INIT(0); // gdMinislot
+	uint32_t NetworkIdleTimeMacroticks INIT(0);
+	uint8_t NetworkManagementVectorLengthBytes INIT(0);
+	uint32_t NumberOfMinislots INIT(0);
+	uint16_t NumberOfStaticSlots INIT(0);
+	uint32_t OffsetCorrectionStartMacroticks INIT(0);
+	uint8_t PayloadLengthOfStaticSlotInWords INIT(0);
+	uint32_t SafetyMarginMacroticks INIT(0);
+
+	double SampleClockPeriodSec INIT(0);
+	bool UseSampleClockPeriodSec INIT(false);
+
+	uint16_t StaticSlotMacroticks INIT(0); // gdStaticSlot
+	uint32_t SymbolWindowMacroticks INIT(0);
+	uint32_t SymbolWindowActionPointOffsetMacroticks INIT(0);
+	uint16_t SyncFrameIDCountMax INIT(0); // gSyncNodeMax
+
+	double TransceiverStandbyDelaySec INIT(false);
+	bool UseTransceiverStandbyDelaySec INIT(0);
+
+	uint8_t TransmissionStartSequenceDurationBits INIT(0); // gdTSSTransmitter
+	uint8_t WakeupRxIdleBits INIT(0);
+	uint8_t WakeupRxLowBits INIT(0);
+	uint16_t WakeupRxWindowBits INIT(0); // gdWakeupSymbolRxWindow
+	uint8_t WakeupTxActiveBits INIT(0); // gdWakeupSymbolTxLow (active = low here)
+	uint8_t WakeupTxIdleBits INIT(0);
+
+#ifdef __cplusplus
+};
+#else
+} neoflexray_cluster_config_t;
+#endif
+
+#ifdef __cplusplus
+} // namespace Cluster
+
+} // namespace FlexRay
+
+} // namespace icsneo
+#endif // __cplusplus
+
+#endif // __FLEXRAYCLUSTER_H_

include/icsneo/device/extensions/flexray/controller.h

@@ -1,13 +1,21 @@
 #ifndef __FLEXRAYCONTROLLER_H_
 #define __FLEXRAYCONTROLLER_H_
 
+#ifdef __cplusplus
+
 #include <cstdint>
 #include <memory>
 #include <chrono>
 #include <mutex>
 #include "icsneo/communication/message/flexray/control/flexraycontrolmessage.h"
+#include "icsneo/communication/message/flexray/flexraymessage.h"
+#include "icsneo/device/extensions/flexray/messagebuffer.h"
 #include "icsneo/device/extensions/flexray/erayregister.h"
 #include "icsneo/device/extensions/flexray/poccommand.h"
+#include "icsneo/device/extensions/flexray/pocstatus.h"
+#include "icsneo/device/extensions/flexray/cluster.h"
+
+#define INIT(x) = x
 
 namespace icsneo {
 
@@ -17,21 +25,46 @@ namespace FlexRay {
 
 class Controller {
 public:
-	Controller(Device& device, uint8_t index) : device(device), index(index) {}
+	Controller(Device& device, uint8_t index, Network net) : device(device), index(index), network(net) {}
-	std::shared_ptr<FlexRayControlMessage> getStatus() const;
 	void _setStatus(std::shared_ptr<FlexRayControlMessage> msg);
 
+	// Begin Public Interface
+	std::shared_ptr<FlexRayControlMessage> getStatus() const;
+
+	const Network& getNetwork() const { return network; }
+
+	struct Configuration; // Forward declaration
+	std::pair<const Cluster::Configuration&, const Controller::Configuration&> getConfiguration() const;
+	void setConfiguration(Cluster::Configuration clConfig, Controller::Configuration coConfig);
+
+	bool getStartWhenGoingOnline() const { return startWhenGoingOnline; }
+	void setStartWhenGoingOnline(bool enable) { startWhenGoingOnline = enable; }
+
 	bool getAllowColdstart() const { return allowColdstart; }
 	void setAllowColdstart(bool enable) { allowColdstart = enable; }
 
 	bool getWakeupBeforeStart() const { return wakeupBeforeStart; }
 	void setWakeupBeforeStart(bool enable) { wakeupBeforeStart = enable; }
 
-	void getReady();
+	void addMessageBuffer(MessageBuffer buffer);
-	void start();
+	void clearMessageBuffers();
+
+	bool wakeup(std::chrono::milliseconds timeout = std::chrono::milliseconds(50));
+	bool configure(std::chrono::milliseconds timeout = std::chrono::milliseconds(2000));
+	bool getReady(std::chrono::milliseconds timeout = std::chrono::milliseconds(2000));
+	bool start(std::chrono::milliseconds timeout = std::chrono::milliseconds(2000));
+
+	bool transmit(const std::shared_ptr<FlexRayMessage>& frmsg);
+
+	bool halt(std::chrono::milliseconds timeout = std::chrono::milliseconds(50));
+	bool freeze(std::chrono::milliseconds timeout = std::chrono::milliseconds(50));
+	bool triggerMTS(std::chrono::milliseconds timeout = std::chrono::milliseconds(200));
+	// End Public Interface
 
 private:
 	bool isPOCBusy() const { return readRegisterOr(ERAYRegister::SUCC1, 0x00000080) & 0x00000080; }
+	bool isClearAllRAMBusy() const { return readRegisterOr(ERAYRegister::MHDS, 0x00000080) & 0x00000080; }
+	bool isInputBufferHostBusy() const { return readRegisterOr(ERAYRegister::IBCR, 0x00008000) & 0x00008000; }
 	std::pair<bool, FlexRay::POCCommand> getCurrentPOCCommand(std::chrono::milliseconds timeout = std::chrono::milliseconds(50)) const;
 	bool setCurrentPOCCommand(
 		FlexRay::POCCommand cmd,
@@ -39,6 +72,14 @@ private:
 		std::chrono::milliseconds timeout = std::chrono::milliseconds(50));
 	bool wasCommandSuccessful(std::chrono::milliseconds timeout = std::chrono::milliseconds(50)) const;
 
+	std::pair<bool, FlexRay::POCStatus> getCurrentPOCStatus(std::chrono::milliseconds timeout = std::chrono::milliseconds(50)) const;
+
+	bool lockConfiguration(std::chrono::milliseconds timeout = std::chrono::milliseconds(150));
+	bool enterConfig(std::chrono::milliseconds timeout = std::chrono::milliseconds(50));
+
+	static uint16_t CalculateHCRC(const MessageBuffer& buf);
+	static uint16_t CalculateCycleFilter(uint8_t baseCycle, uint8_t cycleRepetition);
+
 	std::pair<bool, uint32_t> readRegister(
 		ERAYRegister reg,
 		std::chrono::milliseconds timeout = std::chrono::milliseconds(50)) const;
@@ -60,15 +101,107 @@ private:
 
 	Device& device;
 	uint8_t index;
+	Network network;
 	mutable std::mutex statusLock;
 	mutable std::mutex readRegisterLock;
 	std::shared_ptr<FlexRayControlMessage> status;
+	bool startWhenGoingOnline = false;
 	bool allowColdstart = false;
 	bool wakeupBeforeStart = false;
+
+public:
+#define neoflexray_controller_config_t icsneo::FlexRay::Controller::Configuration
+	struct Configuration {
+
+#else // __cplusplus
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define INIT(x)
+
+	typedef struct {
+#endif // __cplusplus
+
+		uint16_t AcceptStartupRangeMicroticks INIT(0);
+		bool AllowHaltDueToClock INIT(false);
+
+		uint8_t AllowPassiveToActiveCyclePairs INIT(0);
+
+		uint8_t ClusterDriftDamping INIT(0);
+
+		bool ChannelA INIT(false);
+		bool ChannelB INIT(false);
+
+		uint8_t DecodingCorrectionMicroticks INIT(0);
+		uint8_t DelayCompensationAMicroticks INIT(0);
+		uint8_t DelayCompensationBMicroticks INIT(0);
+		uint8_t ExternOffsetCorrectionControl INIT(0);
+		uint8_t ExternRateCorrectionControl INIT(0);
+		uint8_t ExternOffsetCorrectionMicroticks INIT(0);
+		uint8_t ExternRateCorrectionMicroticks INIT(0);
+
+		bool ExternalSync INIT(false);
+		bool UseExternalSync INIT(false);
+
+		bool FallBackInternal INIT(false);
+		bool UseFallBackInternal INIT(false);
+
+		uint16_t KeySlotID INIT(0);
+
+		bool KeySlotOnlyEnabled INIT(false); // pSingleSlotEnabled (TSM)
+		bool KeySlotUsedForStartup INIT(false); // pKeySlotUsedForStartup (TXST)
+		bool KeySlotUsedForSync INIT(false); // pKeySlotUsedForSync (TXSY)
+
+		uint16_t LatestTxMinislot INIT(0);
+		uint32_t ListenTimeout INIT(0);
+
+		uint8_t MacroInitialOffsetA INIT(0); // Valid 2..72
+		uint8_t MacroInitialOffsetB INIT(0); // Valid 2..72
+
+		uint32_t MaximumDynamicPayloadLengthWords INIT(0);
+
+		uint8_t MicroInitialOffsetA INIT(0); // Valid 0..240
+		uint8_t MicroInitialOffsetB INIT(0); // Valid 0..240
+
+		uint32_t MicroPerCycle INIT(0);
+
+		double MicrotickDurationSec INIT(0);
+		bool UseMicrotickDurationSec INIT(false);
+
+		bool MTSOnA INIT(false);
+		bool MTSOnB INIT(false);
+
+		bool NMVectorEarlyUpdate INIT(false);
+		bool UseNMVectorEarlyUpdate INIT(false);
+
+		uint16_t OffsetCorrectionOutMicroticks INIT(0);
+		uint16_t RateCorrectionOutMicroticks INIT(0); // pdMaxDrift and pRateCorrectionOut
+
+		uint32_t SamplesPerMicrotick INIT(0);
+		bool UseSamplesPerMicrotick INIT(false);
+
+		uint16_t SecondKeySlotID INIT(0);
+		bool TwoKeySlotMode INIT(false);
+
+		uint8_t WakeupPattern INIT(0);
+		bool WakeupOnChannelB INIT(false);
+
+#ifndef __cplusplus
+	} neoflexray_controller_config_t;
+#else
+	};
+
+private:
+	bool configDirty = false;
+	Cluster::Configuration clusterConfig;
+	Controller::Configuration controllerConfig;
+	std::vector<MessageBuffer> messageBuffers;
 };
 
 } // namespace FlexRay
 
 } // namespace icsneo
+#endif // __cplusplus
 
 #endif // __FLEXRAYCONTROLLER_H_

include/icsneo/device/extensions/flexray/erayregister.h

@@ -5,6 +5,8 @@
 
 namespace icsneo {
 
+namespace FlexRay {
+
 enum class ERAYRegister : uint32_t {
 	// Customer Registers : Not Part of ERAY, part of FUJITSU FlexRay  ASSP MB88121C
 	VER = 0x0000,// Version Information Register  reset = 0440_79FF  Access = r
@@ -100,6 +102,8 @@ enum class ERAYRegister : uint32_t {
 	NDAT1 = 0x0330,// New Data Register 1  0000_0000  r
 	NDAT2 = 0x0334,// New Data Register 2  0000_0000  r
 
+	ENDN = 0x03F4,// Endian check, 8765_4321  r
+
 	WRHS1 = 0x0500,// Write Header Section Register 1
 	WRHS2 = 0x0504,// Write Header Section Register 2
 	WRHS3 = 0x0508,// Write Header Section Register 3
@@ -111,6 +115,8 @@ enum class ERAYRegister : uint32_t {
 	OBCR = 0x714,
 };
 
+} // namespace FlexRay
+
 } // namespace icsneo
 
 #endif // __ERAYREGISTER_H_

include/icsneo/device/extensions/flexray/extension.h

@@ -16,9 +16,14 @@ class Controller;
 
 class Extension : public DeviceExtension {
 public:
-	Extension(Device& device, uint8_t controllerCount);
+	Extension(Device& device, const std::vector<Network>& controllerNetworks);
 	const char* getName() const override { return "FlexRay"; }
+
+	void onGoOnline() override;
+	void onGoOffline() override;
+
 	void handleMessage(const std::shared_ptr<Message>& message) override;
+	bool transmitHook(const std::shared_ptr<Message>& message, bool& success) override;
 
 	std::shared_ptr<Controller> getController(uint8_t index) const {
 		if(index >= controllers.size())

include/icsneo/device/extensions/flexray/messagebuffer.h

@@ -0,0 +1,52 @@
+#ifndef __FLEXRAYMESSAGEBUFFER_H_
+#define __FLEXRAYMESSAGEBUFFER_H_
+
+#ifdef __cplusplus
+
+#include <cstdint>
+
+namespace icsneo {
+
+namespace FlexRay {
+
+#define INIT(x) = x
+
+#define neoflexray_message_buffer_t icsneo::FlexRay::MessageBuffer
+struct MessageBuffer {
+#else // __cplusplus
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define INIT(x)
+
+typedef struct {
+
+#endif // __cplusplus
+
+	bool isDynamic INIT(false);
+	bool isSync INIT(false); // Must be set if isStartup is set!
+	bool isStartup INIT(false);
+	bool isNMFrame INIT(false);
+	bool isTransmit INIT(false);
+	uint16_t frameID INIT(0);
+	bool channelA INIT(false);
+	bool channelB INIT(false);
+	uint8_t frameLengthBytes INIT(0);
+	uint8_t baseCycle INIT(0);
+	uint8_t cycleRepetition INIT(0);
+	bool continuousMode INIT(false);
+	uint16_t _dataPointer INIT(0); // For internal use
+	uint16_t _id INIT(0); // For internal use
+
+#ifndef __cplusplus
+} neoflexray_message_buffer_t;
+#else
+};
+
+} // namespace FlexRay
+
+} // namespace icsneo
+#endif // __cplusplus
+
+#endif // __FLEXRAYMESSAGEBUFFER_H_

include/icsneo/device/extensions/flexray/pocstatus.h

@@ -14,6 +14,7 @@ enum class POCStatus : uint8_t {
 	NormalPassive = 0x03,
 	Halt = 0x04,
 	MonitorMode = 0x05,
+	Config = 0x0f,
 	WakeupStandby = 0x10,
 	WakeupListen = 0x11,
 	WakeupSend = 0x12,

include/icsneo/device/tree/plasion/plasion.h

@@ -17,11 +17,14 @@ protected:
 		std::unique_ptr<Decoder> decoder
 	) override { return std::make_shared<MultiChannelCommunication>(report, std::move(transport), packetizer, std::move(encoder), std::move(decoder)); }
 
-	// TODO: This is done so that Plasion can still transmit it's basic networks, awaiting VLAN support
+	// TODO This is done so that Plasion can still transmit it's basic networks, awaiting slave VNET support
 	virtual bool isSupportedRXNetwork(const Network&) const override { return true; }
 	virtual bool isSupportedTXNetwork(const Network&) const override { return true; }
 	virtual void setupExtensions() override {
-		addExtension(std::make_shared<FlexRay::Extension>(*this, (uint8_t)2));
+		std::vector<Network> flexRayControllers;
+		flexRayControllers.push_back(Network::NetID::FlexRay);
+		flexRayControllers.push_back(Network::NetID::FlexRay2);
+		addExtension(std::make_shared<FlexRay::Extension>(*this, flexRayControllers));
 	}
 
 	static const std::vector<Network>& GetSupportedNetworks() {
@@ -57,29 +60,26 @@ protected:
 	virtual void setupSupportedRXNetworks(std::vector<Network>& rxNetworks) override {
 		for(auto& netid : GetSupportedNetworks())
 			rxNetworks.emplace_back(netid);
+		// TODO Check configuration for FlexRay ColdStart mode, disable FlexRay 2 if so
 	}
 
-	virtual std::shared_ptr<FlexRay::Controller> getFlexRayControllerByNetwork(const Network& net) const override {
+	virtual std::vector<std::shared_ptr<FlexRay::Controller>> getFlexRayControllers() const override {
-		uint8_t idx = 0xff;
+		// TODO Check configuration for FlexRay Enabled
-		switch(net.getNetID()) {
+
-			case Network::NetID::FlexRay:
-				idx = 0;
-				break;
-			case Network::NetID::FlexRay2:
-				idx = 1;
-				break;
-			default:
-				return Device::getFlexRayControllerByNetwork(net);
-		}
-		
 		auto extension = getExtension<FlexRay::Extension>();
 		if(!extension)
-			return Device::getFlexRayControllerByNetwork(net);
+			return Device::getFlexRayControllers();
 		
-		auto res = extension->getController(idx);
+		std::vector<std::shared_ptr<FlexRay::Controller>> ret;
-		if(!res)
+
-			return Device::getFlexRayControllerByNetwork(net);
+		if(auto ctrl1 = extension->getController(0))
-		return res;
+			ret.push_back(std::move(ctrl1));
+
+		// TODO Check configuration for FlexRay ColdStart mode, FlexRay2 -> FlexRay if so
+		if(auto ctrl2 = extension->getController(1))
+			ret.push_back(std::move(ctrl2));
+
+		return ret;
 	}
 
 public: