Merge 31d4a750d8 into c5bce795c6

An incoming bytestream can be less than 24 bytes, leading to exceptions
when accessing its data (or allocating the vector for its payload).
Perform a size check before trying to decode the bytestream and discard
invalid incoming streams.

Signed-off-by: Francesco Valla <francesco.valla@mta.it>

.gitignore

@@ -15,3 +15,4 @@ third-party/concurrentqueue/tests
 examples/csharp/bin
 examples/csharp/obj
 test/system
+.env

CMakeLists.txt

@@ -292,6 +292,7 @@ set(SRC_FILES
 	disk/plasiondiskreaddriver.cpp
 	disk/extextractordiskreaddriver.cpp
 	disk/fat.cpp
+	disk/diskdetails.cpp
 	disk/vsa/vsa.cpp
 	disk/vsa/vsa02.cpp
 	disk/vsa/vsa03.cpp

api/icsneocpp/event.cpp

@@ -124,6 +124,10 @@ static constexpr const char* LIN_SETTINGS_NOT_AVAILABLE = "LIN settings are not
 static constexpr const char* MODE_NOT_FOUND = "The mode was not found.";
 static constexpr const char* GPTP_NOT_SUPPORTED = "GPTP clock synchronization is not supported on this device.";
 static constexpr const char* SETTING_NOT_AVAILABLE = "Requested a setting that is not available on this device";
+static constexpr const char* DISK_FORMAT_NOT_SUPPORTED = "Disk formatting is not supported on this device.";
+static constexpr const char* DISK_FORMAT_INVALID_COUNT = "Disk format config disk count is mismatched with device disk count.";
+
+
 
 // Transport Errors
 static constexpr const char* FAILED_TO_READ = "A read operation failed.";
@@ -371,6 +375,10 @@ const char* APIEvent::DescriptionForType(Type type) {
 			return SEND_TO_ERROR;
 		case Type::GPTPNotSupported:
 			return GPTP_NOT_SUPPORTED;
+		case Type::DiskFormatNotSupported:
+			return DISK_FORMAT_NOT_SUPPORTED;
+		case Type::DiskFormatInvalidCount:
+			return DISK_FORMAT_INVALID_COUNT;
 	
 		// FTD3XX
 		case Type::FTOK:

bindings/python/icsneopy/api/event.cpp

@@ -36,6 +36,8 @@ void init_event(pybind11::module_& m) {
 		.value("SettingsLengthError", APIEvent::Type::SettingsLengthError)
 		.value("SettingsChecksumError", APIEvent::Type::SettingsChecksumError)
 		.value("SettingsNotAvailable", APIEvent::Type::SettingsNotAvailable)
+		.value("DiskFormatNotSupported", APIEvent::Type::DiskFormatNotSupported)
+		.value("DiskFormatInvalidCount", APIEvent::Type::DiskFormatInvalidCount)
 		.value("SettingsReadOnly", APIEvent::Type::SettingsReadOnly)
 		.value("CANSettingsNotAvailable", APIEvent::Type::CANSettingsNotAvailable)
 		.value("CANFDSettingsNotAvailable", APIEvent::Type::CANFDSettingsNotAvailable)

bindings/python/icsneopy/communication/network.cpp

@@ -123,6 +123,7 @@ void init_network(pybind11::module_& m) {
 		.value("I2C_03", Network::NetID::I2C_03)
 		.value("I2C_04", Network::NetID::I2C_04)
 		.value("ETHERNET_02", Network::NetID::ETHERNET_02)
+		.value("ETHERNET_TX_WRAP", Network::NetID::ETHERNET_TX_WRAP)
 		.value("A2B_01", Network::NetID::A2B_01)
 		.value("A2B_02", Network::NetID::A2B_02)
 		.value("ETHERNET_03", Network::NetID::ETHERNET_03)

bindings/python/icsneopy/device/device.cpp

@@ -17,7 +17,7 @@ void init_device(pybind11::module_& m) {
 		.def("close", &Device::close, pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("describe", &Device::describe)
 		.def("disable_message_polling", &Device::disableMessagePolling, pybind11::call_guard<pybind11::gil_scoped_release>())
-		.def("enable_message_polling", &Device::enableMessagePolling, pybind11::call_guard<pybind11::gil_scoped_release>())
+		.def("enable_message_polling", &Device::enableMessagePolling, pybind11::arg("filter") = std::nullopt, pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("get_current_message_count", &Device::getCurrentMessageCount)
 		.def("get_digital_io", &Device::getDigitalIO, pybind11::arg("type"), pybind11::arg("number"), pybind11::call_guard<pybind11::gil_scoped_release>())
 		.def("get_gptp_status", &Device::getGPTPStatus, pybind11::arg("timeout") = std::chrono::milliseconds(100), pybind11::call_guard<pybind11::gil_scoped_release>())

communication/decoder.cpp

@@ -324,6 +324,17 @@ bool Decoder::decode(std::shared_ptr<Message>& result, const std::shared_ptr<Pac
 							result = GPTPStatus::DecodeToMessage(packet->data, report);
 							return true;
 						}
+						case ExtendedCommand::GetDiskDetails:
+						case ExtendedCommand::DiskFormatProgress: {
+							std::vector<uint8_t> responseBody(
+								packet->data.begin() + sizeof(ExtendedResponseMessage::ResponseHeader),
+								packet->data.end()
+							);
+							auto response = std::make_shared<ExtendedResponseMessage>(resp.command);
+							response->data = std::move(responseBody);
+							result = response;
+							return true;
+						}
 						default:
 							// No defined handler, treat this as a RawMessage
 							break;

communication/encoder.cpp

@@ -35,15 +35,25 @@ bool Encoder::encode(const Packetizer& packetizer, std::vector<uint8_t>& result,
 					auto ethmsg = std::dynamic_pointer_cast<EthernetMessage>(message);
 					if(!ethmsg) {
 						report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error);
-						return false; // The message was not a properly formed EthernetMessage
+						return false;
 					}
 
+					shortFormat = false; // Ensure long-format RED header is added
+					netid = static_cast<uint16_t>(Network::NetID::ETHERNET_TX_WRAP);
 					buffer = &result;
-					if(!HardwareEthernetPacket::EncodeFromMessage(*ethmsg, result, report))
+
+					if(!HardwareEthernetPacket::EncodeFromMessage(*ethmsg, result, report)) {
+						report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error);
 						return false;
+					}
+
+					if(result.empty()) {
+						report(APIEvent::Type::MessageFormattingError, APIEvent::Severity::Error);
+						return false;
+					}
 
 					break;
-				} // End of Network::Type::Ethernet
+				}
 				case Network::Type::CAN:
 				case Network::Type::SWCAN:
 				case Network::Type::LSFTCAN: {
@@ -230,7 +240,6 @@ bool Encoder::encode(const Packetizer& packetizer, std::vector<uint8_t>& result,
 			(uint8_t)(netid >> 8)
 		});
 	}
-
 	result = packetizer.packetWrap(*buffer, shortFormat);
 	return true;
 }

communication/ethernetpacketizer.cpp

@@ -133,6 +133,10 @@ EthernetPacketizer::EthernetPacket::EthernetPacket(const uint8_t* data, size_t s
 
 int EthernetPacketizer::EthernetPacket::loadBytestream(const std::vector<uint8_t>& bytestream) {
 	errorWhileDecodingFromBytestream = 0;
+	if (bytestream.size() < 24) {
+		errorWhileDecodingFromBytestream = 1;
+		return errorWhileDecodingFromBytestream;
+	}
 	for(size_t i = 0; i < 6; i++)
 		destMAC[i] = bytestream[i];
 	for(size_t i = 0; i < 6; i++)

communication/packet/ethernetpacket.cpp

@@ -1,5 +1,5 @@
 #include "icsneo/communication/packet/ethernetpacket.h"
-#include <cstring> // memcpy
+#include <algorithm> // for std::copy
 #include <iostream>
 
 using namespace icsneo;
@@ -7,40 +7,31 @@ using namespace icsneo;
 std::shared_ptr<EthernetMessage> HardwareEthernetPacket::DecodeToMessage(const std::vector<uint8_t>& bytestream, const device_eventhandler_t& report) {
 	const HardwareEthernetPacket* packet = (const HardwareEthernetPacket*)((const void*)bytestream.data());
 	const uint16_t* rawWords = (const uint16_t*)bytestream.data();
-	
 	// Make sure we have enough to read the packet length first
 	if(bytestream.size() < sizeof(HardwareEthernetPacket))
 		return nullptr;
-
 	// packet->Length will also encompass the two uint16_t's at the end of the struct, make sure that at least they are here
 	if(packet->Length < 4)
 		return nullptr;
-	
 	const size_t fcsSize = packet->header.FCS_AVAIL ? 4 : 0;
 	const size_t bytestreamExpectedSize = sizeof(HardwareEthernetPacket) + packet->Length;
 	const size_t bytestreamActualSize = bytestream.size();
 	if(bytestreamActualSize < bytestreamExpectedSize)
 		return nullptr;
-
 	// Check for oversized packets, noting that some devices will send an extra byte to have an even number of bytes
 	if(bytestreamActualSize > bytestreamExpectedSize + 1)
 		report(APIEvent::Type::PacketDecodingError, APIEvent::Severity::EventWarning);
-
 	auto messagePtr = std::make_shared<EthernetMessage>();
 	EthernetMessage& message = *messagePtr;
-
 	message.transmitted = packet->eid.TXMSG;
 	if(message.transmitted)
 		message.description = packet->stats;
-
 	message.preemptionEnabled = packet->header.PREEMPTION_ENABLED;
 	if(message.preemptionEnabled)
 		message.preemptionFlags = (uint8_t)((rawWords[0] & 0x03F8) >> 4);
-
 	message.frameTooShort = packet->header.RUNT_FRAME;
 	if(message.frameTooShort)
 		message.error = true;
-
 	// This timestamp is raw off the device (in timestampResolution increments)
 	// Decoder will fix as it has information about the timestampResolution increments
 	message.timestamp = packet->timestamp.TS;
@@ -48,7 +39,7 @@ std::shared_ptr<EthernetMessage> HardwareEthernetPacket::DecodeToMessage(const s
 	const std::vector<uint8_t>::const_iterator databegin = bytestream.begin() + sizeof(HardwareEthernetPacket);
 	const std::vector<uint8_t>::const_iterator dataend = databegin + packet->Length - fcsSize;
 	message.data.insert(message.data.begin(), databegin, dataend);
-	
+
 	if(fcsSize) {
 		uint32_t& fcs = message.fcs.emplace();
 		std::copy(dataend, dataend + fcsSize, (uint8_t*)&fcs);
@@ -59,41 +50,83 @@ std::shared_ptr<EthernetMessage> HardwareEthernetPacket::DecodeToMessage(const s
 
 bool HardwareEthernetPacket::EncodeFromMessage(const EthernetMessage& message, std::vector<uint8_t>& bytestream, const device_eventhandler_t&) {
 	const size_t unpaddedSize = message.data.size();
-	size_t paddedSize = unpaddedSize;
+	if(unpaddedSize == 0)
-	uint16_t description = message.description;
+		return false;
-
-	if(!message.noPadding && unpaddedSize < 60)
-		paddedSize = 60; // Pad out short messages
-
-	size_t sizeWithHeader = paddedSize + 4; // DescriptionID and Padded Count
 
 	// Description ID Most Significant bit is used to identify preemption frames
+	uint16_t description = message.description;
 	if(description & 0x8000)
 		return false;
 
-	if(message.preemptionEnabled) {
+	const bool preempt = message.preemptionEnabled;
-		sizeWithHeader++; // Make space for the preemption flags
+	// full header including parent
+	const size_t headerByteCount = preempt ? 15 : 14;
+	// local header for netID, description, and flags
+	const size_t localHeader = preempt ? 10 : 9;
+	// allocate space for fcs override
+	const size_t fcsSize = message.fcs ? 4 : 0;
+
+	if(preempt)
 		description |= 0x8000;
+
+	size_t paddedSize = unpaddedSize;
+	if(!message.noPadding && unpaddedSize < 60)
+		paddedSize = 60;
+
+	// size of full payload including optional fcs
+	size_t payloadSize = paddedSize + fcsSize;
+
+
+	// totalBufferSize is local header + ethernet payload and option fcs
+	const size_t totalBufferSize = localHeader + paddedSize + fcsSize;
+
+	bytestream.clear();
+	bytestream.reserve(totalBufferSize);
+
+	// Header size field (little endian)
+	bytestream.push_back(static_cast<uint8_t>(payloadSize & 0xFF));
+	bytestream.push_back(static_cast<uint8_t>((payloadSize >> 8) & 0xFF));
+
+	// Description (big endian)
+	bytestream.push_back(static_cast<uint8_t>(description >> 8));
+	bytestream.push_back(static_cast<uint8_t>(description));
+
+	bytestream.push_back(0x00);
+	bytestream.push_back(static_cast<uint8_t>(headerByteCount));
+
+	// Network ID (little endian)
+	uint16_t realID = static_cast<uint16_t>(message.network.getNetID());
+	bytestream.push_back(static_cast<uint8_t>(realID & 0xFF));
+	bytestream.push_back(static_cast<uint8_t>((realID >> 8) & 0xFF));
+
+	// Flags
+	constexpr uint8_t FLAG_PADDING = 0x01;
+	constexpr uint8_t FLAG_FCS = 0x04;
+	constexpr uint8_t FLAG_PREEMPTION = 0x08;
+
+	uint8_t flags = 0x00;
+	if(!message.noPadding) flags |= FLAG_PADDING;
+	if(message.fcs) flags |= FLAG_FCS;
+	if(message.preemptionEnabled) flags |= FLAG_PREEMPTION;
+
+	bytestream.push_back(flags);
+
+	if(preempt)
+		bytestream.push_back(static_cast<uint8_t>(message.preemptionFlags));
+
+	bytestream.insert(bytestream.end(), message.data.begin(), message.data.end());
+
+	// Only zero-fill when we want padding
+	if(!message.noPadding && unpaddedSize < 60) {
+		size_t paddingNeeded = 60 - unpaddedSize;
+		bytestream.insert(bytestream.end(), paddingNeeded, 0); // Zero-fill for padding
 	}
-	
-	bytestream.reserve(sizeWithHeader + 8); // Also reserve space for the bytes we'll use later on
-	bytestream.resize(sizeWithHeader);
-	size_t index = 0;
 
-	// Padded size, little endian
+	if(message.fcs) {
-	bytestream[index++] = uint8_t(paddedSize);
+		uint32_t fcs = message.fcs.value();
-	bytestream[index++] = uint8_t(paddedSize >> 8);
+		const uint8_t* fcsBytes = reinterpret_cast<const uint8_t*>(&fcs);
-
+		bytestream.insert(bytestream.end(), fcsBytes, fcsBytes + sizeof(fcs));
-	// Description ID, big endian
+	}
-	bytestream[index++] = uint8_t(description >> 8);
-	bytestream[index++] = uint8_t(description);
-
-	// The header is one byte larger if preemption is enabled, shifting the data
-	if(message.preemptionEnabled)
-		bytestream[index++] = message.preemptionFlags;
-
-	// We only copy in the unpadded size, the rest will be 0
-	memcpy(bytestream.data() + index, message.data.data(), unpaddedSize);
 
 	return true;
-}
+}

device/device.cpp

@@ -5,6 +5,7 @@
 #include "icsneo/device/device.h"
 #include "icsneo/device/extensions/deviceextension.h"
 #include "icsneo/disk/fat.h"
+#include "icsneo/communication/message/filter/extendedresponsefilter.h"
 
 #ifdef _MSC_VER
 #pragma warning(disable : 4996) // STL time functions
@@ -104,15 +105,21 @@ std::string Device::describe() const {
 	return ss.str();
 }
 
-bool Device::enableMessagePolling() {
+bool Device::enableMessagePolling(std::optional<MessageFilter> filter) {
 	if(isMessagePollingEnabled()) {// We are already polling
 		report(APIEvent::Type::DeviceCurrentlyPolling, APIEvent::Severity::Error);
 		return false;
 	}
-	messagePollingCallbackID = com->addMessageCallback(std::make_shared<MessageCallback>([this](std::shared_ptr<Message> message) {
+	if(!filter.has_value()) {
+		// If no filter is provided, use a default that includes all messages
+		filter.emplace(MessageFilter());
+		filter->includeInternalInAny = true;
+	}
+	auto callback = std::make_shared<MessageCallback>(*filter, [this](std::shared_ptr<Message> message) {
 		pollingContainer.enqueue(message);
 		enforcePollingMessageLimit();
-	}));
+	});
+	messagePollingCallbackID = com->addMessageCallback(callback);
 	return true;
 }
 
@@ -277,7 +284,7 @@ bool Device::open(OpenFlags flags, OpenStatusHandler handler) {
 		std::condition_variable heartbeatCV;
 		std::mutex receivedMessageMutex;
 		bool receivedMessage = false;
-		auto messageReceivedCallbackID = com->addMessageCallback(std::make_shared<MessageCallback>(filter, [&](std::shared_ptr<Message> message) {
+		auto messageReceivedCallbackID = com->addMessageCallback(std::make_shared<MessageCallback>(filter, [&](std::shared_ptr<Message>) {
 			{
 				std::scoped_lock<std::mutex> lk(receivedMessageMutex);
 				receivedMessage = true;
@@ -3487,3 +3494,129 @@ bool Device::enableNetworkCommunication(bool enable) {
 	}
 	return true;
 }
+
+
+
+bool Device::formatDisk(const DiskDetails& config, const DiskFormatProgress& handler, std::chrono::milliseconds interval) {
+#pragma pack(push, 2)
+	struct DiskFormatProgressResponse {
+		uint16_t state;
+		uint64_t sectorsRemaining;
+	};
+#pragma pack(pop)
+
+	auto diskCount = getDiskCount();
+	
+	if(!diskCount) {
+		report(APIEvent::Type::DiskFormatNotSupported, APIEvent::Severity::Error);
+		return false;
+	}
+	if(config.disks.size() != diskCount) {
+		report(APIEvent::Type::DiskFormatInvalidCount, APIEvent::Severity::Error);
+		return false;
+	}
+
+	std::vector<uint8_t> payload = DiskDetails::Encode(config);
+	if(!com->sendCommand(ExtendedCommand::DiskFormatStart, payload)) {
+		return false;
+	}
+
+	uint64_t sectorsFormatted = 0;
+	uint64_t sectorsTotal = 0;
+	uint16_t lastState = 1;
+	do {
+		std::shared_ptr<Message> response = com->waitForMessageSync(
+			[this](){ 
+				return com->sendCommand(ExtendedCommand::DiskFormatProgress, {}); 
+			},
+			std::make_shared<ExtendedResponseFilter>(ExtendedCommand::DiskFormatProgress),
+			std::chrono::milliseconds(200)
+		);
+
+		if(!response) {
+			report(APIEvent::Type::NoDeviceResponse, APIEvent::Severity::Error);
+			return false;
+		}
+
+		auto extResponse = std::dynamic_pointer_cast<ExtendedResponseMessage>(response);
+		if(!extResponse) {
+			// Should never happen
+			return false;
+		}
+		
+		if(extResponse->data.size() < sizeof(DiskFormatProgressResponse)) {
+			report(APIEvent::Type::BufferInsufficient, APIEvent::Severity::Error);
+			return false;
+		}
+
+		auto* progress = reinterpret_cast<DiskFormatProgressResponse*>(extResponse->data.data());
+		lastState = progress->state;
+
+		if(sectorsTotal == 0) {
+			sectorsTotal = progress->sectorsRemaining;
+		} else {
+			sectorsFormatted = sectorsTotal - progress->sectorsRemaining;
+
+			if(handler) {
+				auto directive = handler(sectorsFormatted, sectorsTotal);
+
+				if(directive == DiskFormatDirective::Stop) {
+					return com->sendCommand(ExtendedCommand::DiskFormatCancel);
+				}
+			}
+		}
+
+		std::this_thread::sleep_for(interval);
+	} while(lastState);
+
+	return true;
+}
+
+bool Device::forceDiskConfigUpdate(const DiskDetails& config) {
+	auto diskCount = getDiskCount();
+	
+	if(!diskCount) {
+		report(APIEvent::Type::DiskFormatNotSupported, APIEvent::Severity::Error);
+		return false;
+	}
+	if(config.disks.size() != diskCount) {
+		report(APIEvent::Type::DiskFormatInvalidCount, APIEvent::Severity::Error);
+		return false;
+	}
+
+	return com->sendCommand(ExtendedCommand::DiskFormatUpdate, DiskDetails::Encode(config));
+}
+
+std::shared_ptr<DiskDetails> Device::getDiskDetails(std::chrono::milliseconds timeout) {
+	if(!supportsDiskFormatting()) {
+		report(APIEvent::Type::DiskFormatNotSupported, APIEvent::Severity::Error);
+		return nullptr;
+	}
+
+	if(!isOpen()) {
+		report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error);
+		return nullptr;
+	}
+
+	std::shared_ptr<Message> response = com->waitForMessageSync(
+		[this](){ 
+			return com->sendCommand(ExtendedCommand::GetDiskDetails, {}); 
+		},
+		std::make_shared<ExtendedResponseFilter>(ExtendedCommand::GetDiskDetails),
+		timeout
+	);
+
+	if(!response) {
+		report(APIEvent::Type::NoDeviceResponse, APIEvent::Severity::Error);
+		return nullptr;
+	}
+
+	auto extResponse = std::dynamic_pointer_cast<ExtendedResponseMessage>(response);
+	if(!extResponse) {
+		// Should never happen
+		return nullptr;
+	}
+
+	return DiskDetails::Decode(extResponse->data, getDiskCount(), report);
+}
+

disk/diskdetails.cpp

@@ -0,0 +1,81 @@
+#include <icsneo/disk/diskdetails.h>
+#include <icsneo/device/idevicesettings.h>
+
+using namespace icsneo;
+
+#pragma pack(push, 2)
+
+struct DISK_STATUS {
+	uint16_t status;
+	uint64_t sectors;
+	uint32_t bytesPerSector;
+};
+
+struct DISK_DETAILS {
+	DISK_SETTINGS settings;
+	uint16_t options;
+	DISK_STATUS status[12];
+};
+
+#pragma pack(pop)
+
+static constexpr uint8_t DISK_FORMAT_FAT32 = 0x01u;
+static constexpr uint8_t DISK_STATUS_PRESENT = 0x01u;
+static constexpr uint8_t DISK_STATUS_INITIALIZED = 0x02u;
+static constexpr uint16_t DISK_DETAILS_FULL_FORMAT = 0x01u;
+
+std::vector<uint8_t> DiskDetails::Encode(const DiskDetails& config) {
+	std::vector<uint8_t> result(sizeof(DISK_DETAILS), 0);
+	DISK_DETAILS* details = reinterpret_cast<DISK_DETAILS*>(result.data());
+	details->settings.disk_layout = static_cast<uint8_t>(config.layout);
+	details->settings.disk_format = DISK_FORMAT_FAT32; // Always FAT32
+	details->options = config.fullFormat ? DISK_DETAILS_FULL_FORMAT : 0;
+	uint32_t& enables = details->settings.disk_enables;
+	enables = 0u;
+	for(size_t i = 0; i < config.disks.size(); i++) {
+		auto& disk = config.disks[i];
+		details->status[i].sectors = disk.sectors;
+		details->status[i].bytesPerSector = disk.bytesPerSector;
+
+		uint16_t& status = details->status[i].status;
+		status = 0u;
+		if(disk.initialized) {
+			status |= DISK_STATUS_INITIALIZED;
+		}
+		if(disk.present) {
+			status |= DISK_STATUS_PRESENT;
+		}
+		if(disk.formatted) {
+			enables |= (1u << i);
+		}
+	}
+
+	return result;
+}
+
+std::shared_ptr<DiskDetails> DiskDetails::Decode(const std::vector<uint8_t>& bytes, size_t diskCount, device_eventhandler_t report) {
+	if(bytes.size() < sizeof(DISK_DETAILS)) {
+		report(APIEvent::Type::BufferInsufficient, APIEvent::Severity::Error);
+		return nullptr;
+	}
+
+	auto result = std::make_shared<DiskDetails>();
+	const DISK_DETAILS* details = reinterpret_cast<const DISK_DETAILS*>(bytes.data());
+	result->layout = static_cast<DiskLayout>(details->settings.disk_layout);
+	result->fullFormat = details->options & DISK_DETAILS_FULL_FORMAT;
+
+	result->disks.reserve(diskCount);
+
+	for(size_t i = 0; i < diskCount; i++) {
+		auto& disk = details->status[i];
+		result->disks.emplace_back();
+		auto& info = result->disks.back();
+		info.present = static_cast<bool>(disk.status & DISK_STATUS_PRESENT);
+		info.initialized = static_cast<bool>(disk.status & DISK_STATUS_INITIALIZED);
+		info.formatted = static_cast<bool>(details->settings.disk_enables & (1u << i));  
+		info.sectors = disk.sectors;
+		info.bytesPerSector = disk.bytesPerSector;
+	}
+
+	return result;
+}

disk/extextractordiskreaddriver.cpp

@@ -32,7 +32,7 @@ std::optional<uint64_t> ExtExtractorDiskReadDriver::readLogicalDiskAligned(Commu
 	return ret;
 }
 
-std::optional<uint64_t> ExtExtractorDiskReadDriver::attemptReadLogicalDiskAligned(Communication& com, device_eventhandler_t report,
+std::optional<uint64_t> ExtExtractorDiskReadDriver::attemptReadLogicalDiskAligned(Communication& com, device_eventhandler_t,
 	uint64_t pos, uint8_t* into, uint64_t amount, std::chrono::milliseconds timeout, MemoryType) {
 	static std::shared_ptr<MessageFilter> NeoMemorySDRead = std::make_shared<MessageFilter>(Network::NetID::NeoMemorySDRead);
 

disk/neomemorydiskdriver.cpp

@@ -58,7 +58,7 @@ std::optional<uint64_t> NeoMemoryDiskDriver::readLogicalDiskAligned(Communicatio
 	return SectorSize;
 }
 
-std::optional<uint64_t> NeoMemoryDiskDriver::writeLogicalDiskAligned(Communication& com, device_eventhandler_t report,
+std::optional<uint64_t> NeoMemoryDiskDriver::writeLogicalDiskAligned(Communication& com, device_eventhandler_t,
 	uint64_t pos, const uint8_t* from, uint64_t amount, std::chrono::milliseconds timeout, MemoryType memType) {
 
 	static std::shared_ptr<MessageFilter> NeoMemoryDone = std::make_shared<MessageFilter>(Network::NetID::NeoMemoryWriteDone);

disk/plasiondiskreaddriver.cpp

@@ -6,7 +6,7 @@
 using namespace icsneo;
 using namespace icsneo::Disk;
 
-std::optional<uint64_t> PlasionDiskReadDriver::readLogicalDiskAligned(Communication& com, device_eventhandler_t report,
+std::optional<uint64_t> PlasionDiskReadDriver::readLogicalDiskAligned(Communication& com, device_eventhandler_t,
 	uint64_t pos, uint8_t* into, uint64_t amount, std::chrono::milliseconds timeout, MemoryType) {
 	static std::shared_ptr<MessageFilter> NeoMemorySDRead = std::make_shared<MessageFilter>(Network::NetID::NeoMemorySDRead);
 

include/icsneo/api/event.h

@@ -113,6 +113,8 @@ public:
 		AppErrorParsingFailed = 0x2055,
 		GPTPNotSupported = 0x2056,
 		SettingNotAvaiableDevice = 0x2057,
+		DiskFormatNotSupported = 0x2058,
+		DiskFormatInvalidCount = 0x2059,
 
 		// Transport Events
 		FailedToRead = 0x3000,

include/icsneo/communication/message/extendedresponsemessage.h

@@ -10,11 +10,12 @@ namespace icsneo {
 
 class ExtendedResponseMessage : public Message {
 public:
-	ExtendedResponseMessage(ExtendedCommand cmd, ExtendedResponse resp)
+	ExtendedResponseMessage(ExtendedCommand cmd, ExtendedResponse resp = ExtendedResponse::OK)
 		: Message(Message::Type::ExtendedResponse), command(cmd), response(resp) {}
 
 	const ExtendedCommand command;
 	const ExtendedResponse response;
+	std::vector<uint8_t> data;
 
 #pragma pack(push, 2)
 	struct ResponseHeader {

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

@@ -15,20 +15,36 @@ namespace icsneo {
 
 class ExtendedResponseFilter : public MessageFilter {
 public:
-	ExtendedResponseFilter(icsneo::ExtendedResponse resp) : MessageFilter(Message::Type::ExtendedResponse), response(resp) {}
+	ExtendedResponseFilter(icsneo::ExtendedCommand cmd) : MessageFilter(Message::Type::ExtendedResponse), command(cmd), response(std::nullopt) {}
+	ExtendedResponseFilter(icsneo::ExtendedResponse resp) : MessageFilter(Message::Type::ExtendedResponse), command(std::nullopt), response(resp) {}
+	ExtendedResponseFilter(icsneo::ExtendedCommand cmd, icsneo::ExtendedResponse resp) 
+		: MessageFilter(Message::Type::ExtendedResponse), command(cmd), response(resp) {}
 
 	bool match(const std::shared_ptr<Message>& message) const override {
 		if(!MessageFilter::match(message)) {
 			return false;
 		}
 		const auto respMsg = std::static_pointer_cast<ExtendedResponseMessage>(message);
-		return respMsg && matchResponse(respMsg->response);
+		return respMsg && matchResponse(respMsg->command, respMsg->response);
 	}
 
 private:
-	icsneo::ExtendedResponse response;
+	std::optional<icsneo::ExtendedCommand> command = std::nullopt;
-	bool matchResponse(icsneo::ExtendedResponse resp) const {
+	std::optional<icsneo::ExtendedResponse> response = std::nullopt;
-		return response == resp;
+
+	bool matchResponse(icsneo::ExtendedCommand cmd, icsneo::ExtendedResponse resp) const {
+		if(command) {
+			if(*command != cmd) {
+				return false;
+			}
+		}
+		if(response) {
+			if(*response != resp) {
+				return false;
+			}
+		}
+
+		return true;
 	}
 };
 

include/icsneo/communication/network.h

@@ -143,6 +143,7 @@ public:
 		I2C_03 = 518, // previously I2C3
 		I2C_04 = 519, // previously I2C4
 		ETHERNET_02 = 520, // previously Ethernet2
+		ETHERNET_TX_WRAP = 521,
 		A2B_01 = 522, // previously A2B1
 		A2B_02 = 523, // previously A2B2
 		ETHERNET_03 = 524, // previously Ethernet3
@@ -591,6 +592,7 @@ public:
 		case NetID::ETHERNET_01:
 		case NetID::ETHERNET_DAQ:
 		case NetID::ETHERNET_02:
+		case NetID::ETHERNET_TX_WRAP:
 		case NetID::ETHERNET_03:
 		case NetID::AE_01:
 		case NetID::AE_02:
@@ -887,6 +889,8 @@ public:
 			return "I2C 04";
 		case NetID::ETHERNET_02:
 			return "Ethernet 02";
+		case NetID::ETHERNET_TX_WRAP:
+			return "Ethernet TX Wrap";
 		case NetID::A2B_01:
 			return "A2B 01";
 		case NetID::A2B_02:

include/icsneo/communication/packet/ethernetpacket.h

@@ -7,6 +7,9 @@
 #include "icsneo/api/eventmanager.h"
 #include <cstdint>
 #include <memory>
+#include <iostream>
+#include <iomanip>
+#include <sstream>
 
 namespace icsneo {
 
@@ -18,30 +21,59 @@ struct HardwareEthernetPacket {
 	static std::shared_ptr<EthernetMessage> DecodeToMessage(const std::vector<uint8_t>& bytestream, const device_eventhandler_t& report);
 	static bool EncodeFromMessage(const EthernetMessage& message, std::vector<uint8_t>& bytestream, const device_eventhandler_t& report);
 
+	// Word 0 - Header flags (offset 0)
 	struct {
 		icscm_bitfield FCS_AVAIL : 1;
 		icscm_bitfield RUNT_FRAME : 1;
-		icscm_bitfield DISABLE_PADDING : 1;
+		icscm_bitfield ENABLE_PADDING : 1;
 		icscm_bitfield PREEMPTION_ENABLED : 1;
 		icscm_bitfield MPACKET_TYPE : 4;
 		icscm_bitfield MPACKET_FRAG_CNT : 2;
-		icscm_bitfield : 6;
+		icscm_bitfield UPDATE_CHECKSUMS : 1;
+		icscm_bitfield FCS_OVERRIDE : 1;
+		icscm_bitfield CRC_ERROR : 1;
+		icscm_bitfield FCS_VERIFIED : 1;
+		icscm_bitfield DISABLE_TX_RECEIPT : 1;
+		icscm_bitfield T1S_ETHERNET : 1;
 	} header;
+
+	// Word 1 - Extended ID flags
 	struct {
 		icscm_bitfield txlen : 12;
 		icscm_bitfield TXMSG : 1;
-		icscm_bitfield : 3;
+		icscm_bitfield TXAborted : 1;
+		icscm_bitfield T1S_SYMBOL : 1;
+		icscm_bitfield T1S_BURST : 1;
 	} eid;
-	icscm_bitfield reserved;
+
-	unsigned char data[8];
+	// Word 2 - T1S Status flags
+	struct {
+		icscm_bitfield TXCollision : 1;
+		icscm_bitfield T1SWake : 1;
+		icscm_bitfield : 14;
+	} t1s_status;
+
+	// Word 3 - T1S Node Information
+	struct {
+		icscm_bitfield T1S_BURST_COUNT : 8;
+		icscm_bitfield T1S_NODE_ID : 8;
+		uint8_t RESERVED[6];
+	} t1s_node;
+
+	// Words 4-7 - Reserved/Padding
 	uint16_t stats;
+
+	// Words 8-11 - Timestamp (offset 14-22)
 	struct {
 		uint64_t TS : 60;
-		uint64_t : 3; // Reserved for future status bits
+		uint64_t : 3;
 		uint64_t IsExtended : 1;
 	} timestamp;
+
+
 	uint16_t NetworkID;
 	uint16_t Length;
+
 };
 
 #pragma pack(pop)
@@ -50,4 +82,4 @@ struct HardwareEthernetPacket {
 
 #endif // __cplusplus
 
-#endif
+#endif // __ETHERNETPACKET_H__

include/icsneo/communication/tc10.h

@@ -33,8 +33,8 @@ typedef enum _neotc10wakestatus_t {
 typedef enum _neotc10sleepstatus_t {
 	ICSNEO_TC10_NO_SLEEP_RECEIVED = 0,
 	ICSNEO_TC10_SLEEP_RECEIVED = 1,
-	ICSNEO_TC10_SLEEP_FAILED = 1,
+	ICSNEO_TC10_SLEEP_FAILED = 2,
-	ICSNEO_TC10_SLEEP_ABORTED = 1,
+	ICSNEO_TC10_SLEEP_ABORTED = 3,
 } neotc10sleepstatus_t;
 
 typedef struct _neotc10status_t {

include/icsneo/device/device.h

@@ -26,6 +26,7 @@
 #include "icsneo/disk/diskreaddriver.h"
 #include "icsneo/disk/diskwritedriver.h"
 #include "icsneo/disk/nulldiskdriver.h"
+#include "icsneo/disk/diskdetails.h"
 #include "icsneo/communication/communication.h"
 #include "icsneo/communication/packetizer.h"
 #include "icsneo/communication/encoder.h"
@@ -205,7 +206,7 @@ public:
 	// Message polling related functions
 
 
-	bool enableMessagePolling();
+	bool enableMessagePolling(std::optional<MessageFilter> filter = std::nullopt);
 	bool disableMessagePolling();
 	bool isMessagePollingEnabled() { return messagePollingCallbackID != 0; };
 	std::pair<std::vector<std::shared_ptr<Message>>, bool> getMessages();
@@ -637,6 +638,22 @@ public:
 	bool unsubscribeLiveData(const LiveDataHandle& handle);
 	bool clearAllLiveData();
 	bool setValueLiveData(std::shared_ptr<LiveDataSetValueMessage> message);
+	
+	enum class DiskFormatDirective : uint8_t {
+		Continue,
+		Stop
+	};
+
+	using DiskFormatProgress = std::function<DiskFormatDirective(uint64_t sectorsFormatted, uint64_t sectorsTotal)>;
+	bool formatDisk(
+		const DiskDetails& config,
+		const DiskFormatProgress& handler = {},
+		std::chrono::milliseconds interval = std::chrono::milliseconds(500) /** Send updates at an interval of 500ms */
+	);
+	bool forceDiskConfigUpdate(const DiskDetails& config); // Forces a disk layout and enables change without formatting
+	std::shared_ptr<DiskDetails> getDiskDetails(std::chrono::milliseconds timeout = std::chrono::milliseconds(100));
+	virtual size_t getDiskCount() const { return 0; }
+	bool supportsDiskFormatting() const { return getDiskCount() != 0; }
 
 	// VSA Read functions
 

include/icsneo/device/idevicesettings.h

@@ -924,7 +924,8 @@ public:
 	template<typename T> T getStructure() const { return *getStructurePointer<T>(); }
 	template<typename T> bool applyStructure(const T& newStructure);
 
-	const size_t& getSize() const { return structSize; }
+	size_t getSize() const { return structSize; }
+	size_t getSerialSize() const { return settings.size(); }
 
 	// if settings are disabled for this device. always false unless constructed null
 	bool disabled = false;

include/icsneo/device/tree/neoviconnect/neoviconnect.h

@@ -70,6 +70,10 @@ protected:
 	}
 
 	bool supportsGPTP() const override { return true; }
+	
+	size_t getDiskCount() const override {
+		return 2;
+	}
 
 };
 

include/icsneo/device/tree/neovifire2/neovifire2.h

@@ -141,6 +141,10 @@ protected:
 	bool supportsEraseMemory() const override {
 		return true;
 	}
+
+	size_t getDiskCount() const override {
+		return 1;
+	}
 };
 
 }

include/icsneo/device/tree/neovifire3/neovifire3.h

@@ -91,6 +91,10 @@ protected:
 		return true;
 	}
 
+	size_t getDiskCount() const override {
+		return 2;
+	}
+
 };
 
 }

include/icsneo/device/tree/neovired2/neovired2.h

@@ -75,6 +75,10 @@ protected:
 	bool supportsEraseMemory() const override {
 		return true;
 	}
+	
+	size_t getDiskCount() const override {
+		return 2;
+	}
 };
 
 }

include/icsneo/device/tree/radepsilon/radepsilon.h

@@ -54,6 +54,10 @@ protected:
 	bool supportsEraseMemory() const override {
 		return true;
 	}
+
+	size_t getDiskCount() const override {
+		return 1;
+	}
 };
 
 }

include/icsneo/device/tree/radgalaxy/radgalaxy.h

@@ -107,6 +107,10 @@ protected:
 	std::optional<MemoryAddress> getCoreminiStartAddressSD() const override {
 		return 0;
 	}
+
+	size_t getDiskCount() const override {
+		return 1;
+	}
 };
 
 }

include/icsneo/device/tree/radgigastar/radgigastar.h

@@ -120,6 +120,10 @@ protected:
 	std::optional<MemoryAddress> getCoreminiStartAddressSD() const override {
 		return 0;
 	}
+
+	size_t getDiskCount() const override {
+		return 1;
+	}
 };
 
 }

include/icsneo/device/tree/radgigastar2/radgigastar2.h

@@ -137,6 +137,11 @@ namespace icsneo
 		{
 			return 0;
 		}
+		
+		size_t getDiskCount() const override 
+		{
+			return 1;
+		}
 	};
 
 }

include/icsneo/disk/diskdetails.h

@@ -0,0 +1,44 @@
+#ifndef __DISKDETAILS_H__
+#define __DISKDETAILS_H__
+
+#ifdef __cplusplus
+
+#include <cstdint>
+#include <vector>
+#include <icsneo/api/eventmanager.h>
+
+namespace icsneo {
+
+enum class DiskLayout : uint8_t {
+	Spanned = 0,
+	RAID0 = 1
+};
+
+struct DiskInfo {
+	bool present; // Disk is connected
+	bool initialized; // Disk is initialized
+	bool formatted; // Disk is formatted
+
+	uint64_t sectors;
+	uint32_t bytesPerSector;
+	
+	uint64_t size() const {
+		return sectors * bytesPerSector;
+	}
+};
+
+struct DiskDetails {
+	DiskLayout layout;
+	bool fullFormat;
+	std::vector<DiskInfo> disks;
+
+	static std::vector<uint8_t> Encode(const DiskDetails& config);
+	static std::shared_ptr<DiskDetails> Decode(const std::vector<uint8_t>& bytes, size_t diskCount, device_eventhandler_t report);
+};
+
+
+} // namespace icsneo
+
+#endif // __cplusplus
+
+#endif