Driver: Add Servd

CMakeLists.txt

@@ -109,6 +109,7 @@ if(LIBICSNEO_BUILD_DOCS)
 endif()
 
 if(WIN32)
+	add_definitions(-DWIN32_LEAN_AND_MEAN -DNOMINMAX -D_CRT_SECURE_NO_WARNINGS)
 	set(PLATFORM_SRC
 		platform/windows/strings.cpp
 		platform/windows/registry.cpp
@@ -306,6 +307,7 @@ set(SRC_FILES
 	disk/vsa/vsa0f.cpp
 	disk/vsa/vsa6a.cpp
 	disk/vsa/vsaparser.cpp
+	platform/servd.cpp
 	${PLATFORM_SRC}
 )
 
@@ -366,7 +368,7 @@ target_include_directories(icsneocpp
 		${CMAKE_CURRENT_SOURCE_DIR}/include
 		${LIBICSNEO_EXTENSION_INCLUDE_PATHS}
 )
-target_link_libraries(icsneocpp PUBLIC Threads::Threads)
+target_link_libraries(icsneocpp PUBLIC Threads::Threads $<$<BOOL:${WIN32}>:ws2_32 iphlpapi>)
 set_property(TARGET icsneocpp PROPERTY POSITION_INDEPENDENT_CODE ON)
 target_compile_features(icsneocpp PUBLIC cxx_auto_type cxx_constexpr cxx_lambdas cxx_nullptr cxx_range_for cxx_rvalue_references cxx_sizeof_member cxx_strong_enums)
 message("Loaded extensions: " ${LIBICSNEO_EXTENSION_TARGETS})
@@ -424,10 +426,10 @@ if(LIBICSNEO_ENABLE_RAW_ETHERNET)
 	if(WIN32)
 		if(LIBICSNEO_NPCAP_INCLUDE_DIR STREQUAL "")
 			target_include_directories(icsneocpp PUBLIC AFTER third-party/winpcap/include)
-			add_definitions(-DWPCAP -DHAVE_REMOTE -DWIN32_LEAN_AND_MEAN)
+			add_definitions(-DWPCAP -DHAVE_REMOTE)
 		else()
 			target_include_directories(icsneocpp PUBLIC AFTER ${LIBICSNEO_NPCAP_INCLUDE_DIR})
-			add_definitions(-DNPCAP -DWIN32_LEAN_AND_MEAN)
+			add_definitions(-DNPCAP)
 		endif()
 	else()
 		find_package(PCAP REQUIRED)

api/icsneocpp/event.cpp

@@ -184,6 +184,19 @@ static constexpr const char* VSA_BYTE_PARSE_FAILURE = "Failure to parse record b
 static constexpr const char* VSA_EXTENDED_MESSAGE_ERROR = "Failure to parse extended message record sequence";
 static constexpr const char* VSA_OTHER_ERROR = "Unknown error in VSA read API.";
 
+// Servd
+static constexpr const char* SERVD_BIND_ERROR = "Error binding socket for Servd communication";
+static constexpr const char* SERVD_NONBLOCK_ERROR = "Error setting non-blocking mode for Servd socket";
+static constexpr const char* SERVD_TRANSCEIVE_ERROR = "Error while sending to or receiving from Servd";
+static constexpr const char* SERVD_OUTDATED_ERROR = "Servd version is lower than client (libicsneo) version, update Servd";
+static constexpr const char* SERVD_INVALID_RESPONSE_ERROR = "Unexpected response from Servd";
+static constexpr const char* SERVD_LOCK_ERROR = "Error locking Servd mutex";
+static constexpr const char* SERVD_SEND_ERROR = "Error sending to Servd";
+static constexpr const char* SERVD_RECV_ERROR = "Error receiving from Servd";
+static constexpr const char* SERVD_POLL_ERROR = "Error polling on Servd socket";
+static constexpr const char* SERVD_NODATA_ERROR = "No data received from Servd";
+static constexpr const char* SERVD_JOIN_MULTICAST_ERROR = "Error joining Servd multicast group";
+
 static constexpr const char* TOO_MANY_EVENTS = "Too many events have occurred. The list has been truncated.";
 static constexpr const char* UNKNOWN = "An unknown internal error occurred.";
 static constexpr const char* INVALID = "An invalid internal error occurred.";
@@ -437,6 +450,30 @@ const char* APIEvent::DescriptionForType(Type type) {
 		case Type::VSAOtherError:
 			return VSA_OTHER_ERROR;
 
+		// Servd
+		case Type::ServdBindError:
+			return SERVD_BIND_ERROR;
+		case Type::ServdNonblockError:
+			return SERVD_NONBLOCK_ERROR;
+		case Type::ServdTransceiveError:
+			return SERVD_TRANSCEIVE_ERROR;
+		case Type::ServdOutdatedError:
+			return SERVD_OUTDATED_ERROR;
+		case Type::ServdInvalidResponseError:
+			return SERVD_INVALID_RESPONSE_ERROR;
+		case Type::ServdLockError:
+			return SERVD_LOCK_ERROR;
+		case Type::ServdSendError:
+			return SERVD_SEND_ERROR;
+		case Type::ServdRecvError:
+			return SERVD_RECV_ERROR;
+		case Type::ServdPollError:
+			return SERVD_POLL_ERROR;
+		case Type::ServdNoDataError:
+			return SERVD_NODATA_ERROR;
+		case Type::ServdJoinMulticastError:
+			return SERVD_JOIN_MULTICAST_ERROR;
+
 		// Other Errors
 		case Type::TooManyEvents:
 			return TOO_MANY_EVENTS;

api/icsneolegacy/icsneolegacy.cpp

@@ -2,8 +2,6 @@
 #error "icsneolegacy.cpp must be compiled with a C++ compiler!"
 #endif
 
-#define NOMINMAX
-
 #define ICSNEOC_MAKEDLL
 #include "icsneo/icsneolegacy.h"
 #include "icsneo/J2534.h"

device/device.cpp

@@ -345,14 +345,14 @@ APIEvent::Type Device::attemptToBeginCommunication() {
 		return getCommunicationNotEstablishedError();
 	}
 
-	if(!com->sendCommand(Command::EnableNetworkCommunication, false))
+	if(!enableNetworkCommunication(false))
 		return getCommunicationNotEstablishedError();
 	std::this_thread::sleep_for(std::chrono::milliseconds(10));
 
-	auto serial = com->getSerialNumberSync(std::chrono::milliseconds(200));
+	auto serial = com->getSerialNumberSync();
 	int i = 0;
 	while(!serial) {
-		serial = com->getSerialNumberSync(std::chrono::milliseconds(200));
+		serial = com->getSerialNumberSync();
 		if(i++ > 5)
 			break;
 	}
@@ -406,7 +406,7 @@ bool Device::close() {
 }
 
 bool Device::goOnline() {
-	if(!com->sendCommand(Command::EnableNetworkCommunication, true))
+	if(!enableNetworkCommunication(true))
 		return false;
 
 	auto startTime = std::chrono::system_clock::now();
@@ -449,29 +449,13 @@ bool Device::goOffline() {
 		return true;
 	}
 
-	if(!com->sendCommand(Command::EnableNetworkCommunication, false))
+	if(!enableNetworkCommunication(false))
 		return false;
 
-	auto startTime = std::chrono::system_clock::now();
-
 	ledState = (latestResetStatus && latestResetStatus->cmRunning) ? LEDState::CoreMiniRunning : LEDState::Offline;
 
 	updateLEDState();
 
-	std::shared_ptr<MessageFilter> filter = std::make_shared<MessageFilter>(Network::NetID::Reset_Status);
-	filter->includeInternalInAny = true;
-
-	// Wait until communication is disabled or 5 seconds, whichever comes first
-	while((std::chrono::system_clock::now() - startTime) < std::chrono::seconds(5)) {
-		if(latestResetStatus && !latestResetStatus->comEnabled)
-			break;
-
-		if(!com->sendCommand(Command::RequestStatusUpdate))
-			return false;
-
-		com->waitForMessageSync(filter, std::chrono::milliseconds(100));
-	}
-
 	online = false;
 
 	return true;
@@ -3427,3 +3411,16 @@ bool Device::writeMACsecConfig(const MACsecMessage& message, uint16_t binaryInde
 
 	return writeBinaryFile(raw, binaryIndex);
 }
+
+bool Device::enableNetworkCommunication(bool enable) {
+	bool sendMsg = false;
+	if(!com->driver->enableCommunication(enable, sendMsg)) {
+		return false;
+	}
+	if(sendMsg) {
+		if(!com->sendCommand(Command::EnableNetworkCommunication, enable)) {
+			return false;
+		}
+	}
+	return true;
+}

device/devicefinder.cpp

@@ -2,6 +2,7 @@
 #include "icsneo/platform/devices.h"
 #include "icsneo/device/founddevice.h"
 #include "generated/extensions/builtin.h"
+#include "icsneo/platform/servd.h"
 
 #ifdef ICSNEO_ENABLE_FIRMIO
 #include "icsneo/platform/firmio.h"
@@ -63,6 +64,9 @@ std::vector<std::shared_ptr<Device>> DeviceFinder::FindAll() {
 	static std::vector<FoundDevice> newDriverFoundDevices;
 	newDriverFoundDevices.clear();
 
+	if(Servd::Enabled()) {
+		Servd::Find(newDriverFoundDevices);
+	} else {
 		#ifdef ICSNEO_ENABLE_FIRMIO
 		FirmIO::Find(newDriverFoundDevices);
 		#endif
@@ -86,6 +90,7 @@ std::vector<std::shared_ptr<Device>> DeviceFinder::FindAll() {
 		#ifdef ICSNEO_ENABLE_FTD3XX
 		FTD3XX::Find(newDriverFoundDevices);
 		#endif
+	}
 
 	// Weak because we don't want to keep devices open if they go out of scope elsewhere
 	static std::vector<std::weak_ptr<Device>> foundDevices;

examples/c/legacy/deviceSettings/main.c

@@ -11,7 +11,7 @@ e.g. pSettings.Settings.red2 -> pSettings.Settings.fire3
 #include <stdbool.h>
 
 #if defined _WIN32
-	#include "icsneo/platform/windows.h"
+	#include <windows.h>
 	#define SLEEP(msecs) Sleep(msecs)
 #elif defined (__unix__) || (defined (__APPLE__) && defined (__MACH__))
 	#include <time.h>

examples/c/legacy/lin/main.c

@@ -5,7 +5,7 @@
 #include <stdbool.h>
 
 #if defined _WIN32
-	#include "icsneo/platform/windows.h"
+	#include <windows.h>
 	#define SLEEP(msecs) Sleep(msecs)
 #elif defined (__unix__) || (defined (__APPLE__) && defined (__MACH__))
 	#include <time.h>

include/icsneo/api/event.h

@@ -174,6 +174,19 @@ public:
 		VSAExtendedMessageError = VSABufferCorrupted + 5,
 		VSAOtherError = VSABufferCorrupted + 6,
 
+		// Servd
+		ServdBindError = 0x6000,
+		ServdNonblockError = ServdBindError + 1,
+		ServdTransceiveError = ServdBindError + 2,
+		ServdOutdatedError = ServdBindError + 3,
+		ServdInvalidResponseError = ServdBindError + 4,
+		ServdLockError = ServdBindError + 5,
+		ServdSendError = ServdBindError + 6,
+		ServdRecvError = ServdBindError + 7,
+		ServdPollError = ServdBindError + 8,
+		ServdNoDataError = ServdBindError + 9,
+		ServdJoinMulticastError = ServdBindError + 10,
+
 		NoErrorFound = 0xFFFFFFFD,
 		TooManyEvents = 0xFFFFFFFE,
 		Unknown = 0xFFFFFFFF

include/icsneo/communication/driver.h

@@ -37,6 +37,8 @@ public:
 	bool readAvailable() { return readBuffer.size() > 0; }
 	RingBuffer& getReadBuffer() { return readBuffer; }
 
+	virtual bool enableCommunication(bool /* enable */, bool& sendMsg) { sendMsg = true; return true; }
+
 	device_eventhandler_t report;
 
 	size_t writeQueueSize = 50;

include/icsneo/device/device.h

@@ -1012,6 +1012,8 @@ private:
 	 * @return The size of the vsa log files on the disk
 	 */
 	std::optional<uint64_t> getVSADiskSize();
+
+	bool enableNetworkCommunication(bool enable);
 };
 
 }

include/icsneo/platform/servd.h

@@ -0,0 +1,46 @@
+#ifndef __SERVD_H_
+#define __SERVD_H_
+
+#ifdef __cplusplus
+
+#include <optional>
+#include <thread>
+#include <unordered_set>
+
+#include "icsneo/device/neodevice.h"
+#include "icsneo/communication/driver.h"
+#include "icsneo/device/founddevice.h"
+#include "icsneo/platform/socket.h"
+
+namespace icsneo {
+
+class Servd : public Driver {
+public:
+	static void Find(std::vector<FoundDevice>& foundDevices);
+	static bool Enabled();
+	Servd(const device_eventhandler_t& err, neodevice_t& forDevice, const std::unordered_set<std::string>& availableDrivers);
+	~Servd() override;
+	bool open() override;
+	bool isOpen() override;
+	bool close() override;
+	bool faa(const std::string& key, int32_t inc, int32_t& orig);
+	bool enableCommunication(bool enable, bool& sendMsg) override;
+private:
+	void alive();
+	void read(Address&& address);
+	void write(Address&& address);
+	neodevice_t& device;
+	std::thread aliveThread; // makes sure the client and server are healthy
+	std::thread writeThread;
+	std::thread readThread;
+	Socket messageSocket;
+	bool opened = false;
+	bool comEnabled = false;
+	std::string driver;
+};
+
+}
+
+#endif // __cplusplus
+
+#endif

include/icsneo/platform/socket.h

@@ -0,0 +1,208 @@
+#ifndef __SOCKET_H_
+#define __SOCKET_H_
+
+#ifdef __cplusplus
+
+#ifdef _WIN32
+#include <windows.h>
+#include <winsock2.h>
+#include <ws2tcpip.h>
+#else
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <poll.h>
+#endif
+
+#include <string>
+#include <chrono>
+
+namespace icsneo {
+
+#ifdef _WIN32
+class WSA {
+public:
+	WSA() {
+		// TODO: add error checking
+		WSAStartup(MAKEWORD(2, 2), &wsaData);
+	}
+	~WSA() {
+		WSACleanup();
+	}
+private:
+	WSADATA wsaData;
+};
+#endif
+
+class Address {
+public:
+	Address() = default;
+	Address(const char* ip, uint16_t port)
+		: _ip(ip), _port(port)
+	{
+		_sockaddr.sin_family = AF_INET;
+		inet_pton(AF_INET, ip, &_sockaddr.sin_addr);
+		_sockaddr.sin_port = htons(port);
+	}
+	Address(sockaddr_in& sockaddr)
+		: _sockaddr(sockaddr)
+	{
+		char cip[INET_ADDRSTRLEN];
+		inet_ntop(AF_INET, &sockaddr.sin_addr, cip, sizeof(cip));
+		_ip = cip;
+		_port = ntohs(sockaddr.sin_port);
+	}
+	const std::string& ip() const { return _ip; }
+	const uint16_t& port() const { return _port; }
+	const sockaddr_in& sockaddr() const { return _sockaddr; }
+private:
+	std::string _ip;
+	uint16_t _port;
+	sockaddr_in _sockaddr;
+};
+
+class Socket {
+public:
+	#ifdef _WIN32
+	using SocketHandleType = SOCKET;
+	#else
+	using SocketHandleType = int;
+	#endif
+	
+	Socket() {
+		#ifdef _WIN32
+		static WSA wsa;
+		#endif
+		mFD = socket(AF_INET, SOCK_DGRAM, 0);
+	}
+	
+	~Socket() {
+		#ifdef _WIN32
+		closesocket(mFD);
+		#else
+		close(mFD);
+		#endif
+	}
+	
+	bool set_reuse(bool value) {
+		int ival = value;
+		return ::setsockopt(mFD, SOL_SOCKET, SO_REUSEADDR, (const char*)&ival, sizeof(ival)) != -1;
+	}
+	
+	bool set_nonblocking() {
+		#ifdef _WIN32
+		u_long nonblock = 1;
+		return ioctlsocket(mFD, FIONBIO, &nonblock) != SOCKET_ERROR;
+		#else
+		return fcntl(mFD, F_SETFL, fcntl(mFD, F_GETFL, 0) | O_NONBLOCK) != -1;
+		#endif
+	}
+	
+	bool bind(const Address& at) {
+		return ::bind(mFD, (sockaddr*)&at.sockaddr(), sizeof(sockaddr_in)) != -1;
+	}
+	
+	bool poll(const std::chrono::milliseconds& timeout, bool& in) {
+		#ifdef _WIN32
+		WSAPOLLFD pfd;
+		pfd.fd = mFD;
+		pfd.events = POLLIN;
+		if (::WSAPoll(&pfd, 1, static_cast<int>(timeout.count())) == SOCKET_ERROR) {
+			return false;
+		}
+		in = pfd.revents & POLLIN;
+		return true;
+		#else
+		struct pollfd pfd;
+		pfd.fd = mFD;
+		pfd.events = POLLIN;
+		pfd.revents = 0;
+		if (::poll(&pfd, 1, static_cast<int>(timeout.count())) == -1) {
+			return false;
+		}
+		in = pfd.revents & POLLIN;
+		return true;
+		#endif
+	}
+	
+	bool sendto(const void* buffer, size_t size, const Address& to) {
+		size_t totalSent = 0;
+		do {
+			const auto sent = ::sendto(mFD, (const char*)buffer, (int)size, 0, (sockaddr*)&to.sockaddr(), sizeof(sockaddr_in));
+			if (sent == -1) {
+				return false;
+			}
+			totalSent += sent;
+		} while (totalSent < size);
+		return true;
+	}
+	
+	bool recvfrom(void* buffer, size_t& size, Address& from) {
+		sockaddr_in addr;
+		socklen_t addLen = sizeof(addr);
+		const auto read = ::recvfrom(mFD, (char*)buffer, (int)size, 0, (sockaddr*)&addr, &addLen);
+		if (read == -1) {
+			return false;
+		}
+		size = read;
+		from = Address(addr);
+		return true;
+	}
+	
+	bool recv(void* buffer, size_t& size) {
+		const auto read = ::recv(mFD, (char*)buffer, (int)size, 0);
+		if (read == -1) {
+			return false;
+		}
+		size = read;
+		return true;
+	}
+
+	template<typename REQ, typename RES>
+	bool transceive(const Address& to, REQ&& request, RES&& response, const std::chrono::milliseconds& timeout) {
+		if(!sendto(request.data(), request.size(), to)) {
+			return false;
+		}
+		bool hasData;
+		if(!poll(timeout, hasData)) {
+			return false;
+		}
+		if(!hasData) {
+			return false;
+		}
+		size_t responseSize = response.size();
+		if(!recv(response.data(), responseSize)) {
+			return false;
+		}
+		response.resize(responseSize);
+		return true;
+	}
+	
+	bool address(Address& address) const {
+		sockaddr_in sin;
+		socklen_t len = sizeof(sin);
+		getsockname(mFD, (sockaddr*)&sin, &len);
+		address = Address(sin);
+		return true;
+	}
+	
+	bool join_multicast(const std::string& interfaceIP, const std::string& multicastIP) {
+		ip_mreq mreq;
+		inet_pton(AF_INET, interfaceIP.c_str(), &mreq.imr_interface);
+		inet_pton(AF_INET, multicastIP.c_str(), &mreq.imr_multiaddr);
+		return setsockopt(mFD, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char*)&mreq, sizeof(mreq)) == 0;
+	}
+	
+	operator bool() const { return mFD != -1; }
+	operator SocketHandleType() const { return mFD; }
+private:
+	SocketHandleType mFD;
+};
+
+} // namespace icsneo
+
+#endif // __cplusplus
+
+#endif // __SOCKET_H_

include/icsneo/platform/windows.h

@@ -1,8 +0,0 @@
-#ifndef __PLATFORM_WINDOWS_H_
-#define __PLATFORM_WINDOWS_H_
-
-#if defined _WIN32
-#include "icsneo/platform/windows/windows.h"
-#endif
-
-#endif

include/icsneo/platform/windows/dynamiclib.h

@@ -1,7 +1,7 @@
 #ifndef __DYNAMICLIB_WINDOWS_H_
 #define __DYNAMICLIB_WINDOWS_H_
 
-#include "icsneo/platform/windows.h"
+#include <windows.h>
 
 #ifndef ICSNEOC_BUILD_STATIC
 #ifdef ICSNEOC_MAKEDLL

include/icsneo/platform/windows/internal/pcapdll.h

@@ -3,7 +3,7 @@
 
 #ifdef __cplusplus
 
-#include "icsneo/platform/windows.h"
+#include <windows.h>
 #include <pcap.h>
 #include <memory>
 

include/icsneo/platform/windows/windows.h

@@ -1,13 +0,0 @@
-// Include Windows.h with as few annoying defines as possible
-
-#define NOMINMAX
-#ifndef WIN32_LEAN_AND_MEAN
-#define LAM_DEFINED
-#define WIN32_LEAN_AND_MEAN
-#endif
-#include <Windows.h>
-#ifdef LAM_DEFINED
-#undef LAM_DEFINED
-#undef WIN32_LEAN_AND_MEAN
-#endif
-#undef NOMINMAX

platform/servd.cpp

@@ -0,0 +1,314 @@
+#include "icsneo/platform/servd.h"
+
+using namespace icsneo;
+
+#define SERVD_VERSION 1
+
+static const Address SERVD_ADDRESS = Address("127.0.0.1", 26741);
+static const std::string SERVD_VERSION_STR = std::to_string(SERVD_VERSION);
+
+bool Servd::Enabled() {
+	char* enabled = std::getenv("LIBICSNEO_USE_SERVD");
+	return enabled ? enabled[0] == '1' : false;
+}
+
+std::vector<std::string_view> split(const std::string_view& str, char delim = ' ')
+{
+	std::vector<std::string_view> ret;
+	size_t tail = 0;
+	size_t head = 0;
+	while (head < str.size()) {
+		if (str[head] == delim) {
+			ret.emplace_back(&str[tail], head - tail);
+			tail = head + 1;
+		}
+		++head;
+	}
+	ret.emplace_back(&str[tail], head - tail);
+	return ret;
+}
+
+void Servd::Find(std::vector<FoundDevice>& found) {
+	Socket socket;
+	if(!socket.set_nonblocking()) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdNonblockError, APIEvent::Severity::Error);
+		return;
+	}
+	if(!socket.bind(Address("127.0.0.1", 0))) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdBindError, APIEvent::Severity::Error);
+		return;
+	}
+	std::string response;
+
+	response.resize(512);
+	const std::string version_request = SERVD_VERSION_STR + " version";
+	if(!socket.transceive(SERVD_ADDRESS, version_request, response, std::chrono::milliseconds(5000))) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdTransceiveError, APIEvent::Severity::Error);
+		return;
+	}
+
+	if(std::stoll(response) < SERVD_VERSION) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdOutdatedError, APIEvent::Severity::Error);
+		return;
+	}
+
+	response.resize(512);
+	const std::string find_request = SERVD_VERSION_STR + " find";
+	if(!socket.transceive(SERVD_ADDRESS, find_request, response, std::chrono::milliseconds(5000))) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdTransceiveError, APIEvent::Severity::Error);
+		return;
+	}
+	const auto lines = split(response, '\n');
+	for(auto&& line : lines) {
+		const auto cols = split(line, ' ');
+		if(cols.size() < 2) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdInvalidResponseError, APIEvent::Severity::Error);
+			continue;
+		}
+		const auto& serial = cols[0];
+		std::unordered_set<std::string> drivers;
+		for (size_t i = 1; i < cols.size(); ++i) {
+			drivers.emplace(cols[i]);
+		}
+		auto& newFound = found.emplace_back();
+		std::copy(serial.begin(), serial.end(), newFound.serial);
+		newFound.makeDriver = [=](device_eventhandler_t err, neodevice_t& forDevice) {
+			return std::make_unique<Servd>(err, forDevice, drivers);
+		};
+	}
+}
+
+Servd::Servd(const device_eventhandler_t& err, neodevice_t& forDevice, const std::unordered_set<std::string>& availableDrivers) :
+	Driver(err), device(forDevice) {
+	messageSocket.set_nonblocking();
+	messageSocket.bind(Address("127.0.0.1", 0));
+	if(availableDrivers.count("dxx")) {
+		driver = "dxx"; // prefer USB over Ethernet
+	} else if(availableDrivers.count("cab")) {
+		driver = "cab"; // prefer CAB over TCP
+	} else if(availableDrivers.count("tcp")) {
+		driver = "tcp";
+	} else if(availableDrivers.count("vcp")) {
+		driver = "vcp";
+	} else {
+		// just take the first driver
+		driver = *availableDrivers.begin();
+	}
+}
+
+Servd::~Servd() {
+	close();
+}
+
+bool Servd::open() {
+	const std::string request = SERVD_VERSION_STR + " open " + std::string(device.serial) + " " + driver;
+	std::string response;
+	response.resize(512);
+	if(!messageSocket.transceive(SERVD_ADDRESS, request, response, std::chrono::milliseconds(5000))) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdTransceiveError, APIEvent::Severity::Error);
+		return false;
+	}
+	const auto tokens = split(response);
+	if(tokens.size() != 4) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdInvalidResponseError, APIEvent::Severity::Error);
+		return false;
+	}
+	aliveThread = std::thread(&Servd::alive, this);
+	readThread = std::thread(&Servd::read, this, Address{tokens[2].data(), (uint16_t)std::stol(tokens[3].data())});
+	writeThread = std::thread(&Servd::write, this, Address{tokens[0].data(), (uint16_t)std::stol(tokens[1].data())});
+	opened = true;
+	return true;
+}
+
+bool Servd::isOpen() {
+	return opened;
+}
+
+bool Servd::close() {
+	setIsClosing(true);
+	if(aliveThread.joinable()) {
+		aliveThread.join();
+	}
+	if(readThread.joinable()) {
+		readThread.join();
+	}
+	if(writeThread.joinable()) {
+		writeThread.join();
+	}
+	if(isOpen()) {
+		const std::string request = SERVD_VERSION_STR + " close " + std::string(device.serial);
+		messageSocket.sendto(request.data(), request.size(), SERVD_ADDRESS);
+	}
+	opened = false;
+	return true;
+}
+
+bool Servd::enableCommunication(bool enable, bool& sendMsg) {
+	const std::string serialString(device.serial);
+	{
+		const std::string request = SERVD_VERSION_STR + " lock " + serialString + " com 1000";
+		std::string response;
+		response.resize(1);
+		bool locked = false;
+		const auto timeout = std::chrono::steady_clock::now() + std::chrono::seconds(1);
+		do {
+			if(!messageSocket.transceive(SERVD_ADDRESS, request, response, std::chrono::milliseconds(5000))) {
+				return false;
+			}
+			locked = response == "1" ? true : false;
+			if(locked) {
+				break;
+			}
+			std::this_thread::sleep_for(std::chrono::milliseconds(100));
+		} while (std::chrono::steady_clock::now() < timeout);
+		if(!locked) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdLockError, APIEvent::Severity::Error);
+			return false;
+		}
+	}
+	uint64_t com = 0;
+	{
+		const std::string request = SERVD_VERSION_STR + " load " + serialString + " com";
+		std::string response;
+		response.resize(20);
+		if(!messageSocket.transceive(SERVD_ADDRESS, request, response, std::chrono::milliseconds(5000))) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdTransceiveError, APIEvent::Severity::Error);
+			return false;
+		}
+		com = response.empty() ? 0 : std::stoll(response);
+	}
+	sendMsg = false;
+	if(enable) {
+		sendMsg = true; // always send enable com, we want the status message(s)
+	} else {
+		if(com == 0) {
+			sendMsg = true; // we're the only client, safe to send disable
+		} else if(com == 1 && comEnabled) {
+			sendMsg = true; // we're the last client and we have com enabled, disable
+		}
+	}
+	if(comEnabled != enable) {
+		com += enable ? 1 : -1;
+		const std::string request = SERVD_VERSION_STR + " store " + serialString  + " com " + std::to_string(com);
+		if(!messageSocket.sendto(request.data(), request.size(), SERVD_ADDRESS)) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdSendError, APIEvent::Severity::Error);
+			return false;
+		}
+	}
+	comEnabled = enable;
+	{
+		const std::string request = SERVD_VERSION_STR + " unlock " + serialString + " com";
+		if(!messageSocket.sendto(request.data(), request.size(), SERVD_ADDRESS)) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdSendError, APIEvent::Severity::Error);
+			return false;
+		}
+	}
+	return true;
+}
+
+void Servd::alive() {
+	Socket socket;
+	socket.set_nonblocking();
+	socket.bind(Address("127.0.0.1", 0));
+	const std::string statusRequest = SERVD_VERSION_STR + " status " + std::string(device.serial);
+	std::string statusResponse;
+	statusResponse.resize(8);
+	while(!isDisconnected() && !isClosing()) {
+		if(!socket.sendto(statusRequest.data(), statusRequest.size(), {"127.0.0.1", 26741})) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdSendError, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		bool hasData;
+		if(!socket.poll(std::chrono::milliseconds(2000), hasData)) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdPollError, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		if(!hasData) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdNoDataError, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		size_t statusResponseSize = statusResponse.size();
+		if(!socket.recv(statusResponse.data(), statusResponseSize)) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdRecvError, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		statusResponse.resize(statusResponseSize);
+		if(statusRequest == "closed") {
+			EventManager::GetInstance().add(APIEvent::Type::DeviceDisconnected, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		if(statusResponse != "open") {
+			EventManager::GetInstance().add(APIEvent::Type::ServdInvalidResponseError, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+	}
+}
+
+void Servd::read(Address&& address) {
+	Socket socket;
+	socket.set_nonblocking();
+	socket.set_reuse(true);
+	#ifdef _WIN32
+	if(!socket.bind(Address("127.0.0.1", address.port()))) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdBindError, APIEvent::Severity::Error);
+		setIsDisconnected(true);
+		return;
+	}
+	#else
+	if(!socket.bind(Address(address.ip().c_str(), address.port()))) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdBindError, APIEvent::Severity::Error);
+		setIsDisconnected(true);
+		return;
+	}
+	#endif
+	if(!socket.join_multicast("127.0.0.1", address.ip())) {
+		EventManager::GetInstance().add(APIEvent::Type::ServdJoinMulticastError, APIEvent::Severity::Error);
+		setIsDisconnected(true);
+		return;
+	}
+
+	std::vector<uint8_t> buf(65535);
+	while(!isDisconnected() && !isClosing()) {
+		bool hasData;
+		if(!socket.poll(std::chrono::milliseconds(100), hasData)) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdPollError, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		if(!hasData) {
+			continue;
+		}
+		size_t bufSize = buf.size();
+		if(!socket.recv(buf.data(), bufSize)) {
+			EventManager::GetInstance().add(APIEvent::Type::ServdRecvError, APIEvent::Severity::Error);
+			setIsDisconnected(true);
+			return;
+		}
+		pushRx(buf.data(), bufSize);
+	}
+}
+
+void Servd::write(Address&& address) {
+	Socket socket;
+	socket.bind(Address("127.0.0.1", 0));
+	WriteOperation writeOp;
+	while(!isDisconnected() && !isClosing()) {
+		if(!writeQueue.wait_dequeue_timed(writeOp, std::chrono::milliseconds(100))) {
+			continue;
+		}
+		if(!isClosing()) {
+			if(!socket.sendto(writeOp.bytes.data(), writeOp.bytes.size(), address)) {
+				EventManager::GetInstance().add(APIEvent::Type::ServdSendError, APIEvent::Severity::Error);
+				setIsDisconnected(true);
+				return;
+			}
+		}
+	}
+}

platform/windows/registry.cpp

@@ -1,6 +1,7 @@
 #include "icsneo/platform/windows/registry.h"
 #include "icsneo/platform/windows/strings.h"
-#include "icsneo/platform/windows.h"
+
+#include <windows.h>
 #include <codecvt>
 #include <vector>
 #include <locale>