FirmIO: Initial commit

CMakeLists.txt

@@ -8,6 +8,12 @@ option(LIBICSNEO_BUILD_ICSNEOC "Build dynamic C library" ON)
 option(LIBICSNEO_BUILD_ICSNEOC_STATIC "Build static C library" ON)
 option(LIBICSNEO_BUILD_ICSNEOLEGACY "Build icsnVC40 compatibility library" ON)
 set(LIBICSNEO_NPCAP_INCLUDE_DIR "" CACHE STRING "Npcap include directory; set to build with Npcap")
+
+# Device Drivers
+# You almost certainly don't want firmio for your build,
+# it is only relevant for communication between Linux and
+# CoreMini from the onboard processor of the device.
+option(LIBICSNEO_ENABLE_FIRMIO "Enable communication between Linux and CoreMini within the same device" OFF)
 option(LIBICSNEO_ENABLE_RAW_ETHERNET "Enable devices which communicate over raw ethernet" ON)
 option(LIBICSNEO_ENABLE_CDCACM "Enable devices which communicate over USB CDC ACM" ON)
 option(LIBICSNEO_ENABLE_FTDI "Enable devices which communicate over USB FTDI2XX" ON)
@@ -107,6 +113,12 @@ if(${CMAKE_SYSTEM_NAME} STREQUAL "Windows")
 else() # Darwin or Linux
 	set(PLATFORM_SRC)
 
+	if(LIBICSNEO_ENABLE_FIRMIO)
+		list(APPEND PLATFORM_SRC
+			platform/posix/firmio.cpp
+		)
+	endif()
+
 	if(LIBICSNEO_ENABLE_RAW_ETHERNET)
 		list(APPEND PLATFORM_SRC
 			platform/posix/pcap.cpp
@@ -249,6 +261,9 @@ 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})
 target_link_libraries(icsneocpp PUBLIC ${LIBICSNEO_EXTENSION_TARGETS})
+if(LIBICSNEO_ENABLE_FIRMIO)
+	target_compile_definitions(icsneocpp PRIVATE ICSNEO_ENABLE_FIRMIO)
+endif()
 if(LIBICSNEO_ENABLE_RAW_ETHERNET)
 	target_compile_definitions(icsneocpp PRIVATE ICSNEO_ENABLE_RAW_ETHERNET)
 endif()

communication/driver.cpp

@@ -45,17 +45,18 @@ bool Driver::write(const std::vector<uint8_t>& bytes) {
 	}
 
 	if(writeBlocks) {
-		if(writeQueue.size_approx() > writeQueueSize)
+		if(writeQueueFull()) {
-			while(writeQueue.size_approx() > (writeQueueSize * 3 / 4))
+			while(writeQueueAlmostFull()) // Wait until we have some decent amount of space
 				std::this_thread::sleep_for(std::chrono::milliseconds(10));
+		}
 	} else {
-		if(writeQueue.size_approx() > writeQueueSize) {
+		if(writeQueueFull()) {
 			report(APIEvent::Type::TransmitBufferFull, APIEvent::Severity::Error);
 			return false;
 		}
 	}
 
-	bool ret = writeQueue.enqueue(WriteOperation(bytes));
+	const bool ret = writeInternal(bytes);
 	if(!ret)
 		report(APIEvent::Type::Unknown, APIEvent::Severity::Error);
 

device/devicefinder.cpp

@@ -3,6 +3,10 @@
 #include "icsneo/device/founddevice.h"
 #include "generated/extensions/builtin.h"
 
+#ifdef ICSNEO_ENABLE_FIRMIO
+#include "icsneo/platform/firmio.h"
+#endif
+
 #ifdef ICSNEO_ENABLE_RAW_ETHERNET
 #include "icsneo/platform/pcap.h"
 #endif
@@ -39,6 +43,10 @@ std::vector<std::shared_ptr<Device>> DeviceFinder::FindAll() {
 	static std::vector<FoundDevice> driverFoundDevices;
 	driverFoundDevices.clear();
 
+	#ifdef ICSNEO_ENABLE_FIRMIO
+	FirmIO::Find(driverFoundDevices);
+	#endif
+
 	#ifdef ICSNEO_ENABLE_RAW_ETHERNET
 	PCAP::Find(driverFoundDevices);
 	#endif

include/icsneo/communication/driver.h

@@ -24,9 +24,9 @@ public:
 	virtual void awaitModeChangeComplete() {}
 	virtual bool isDisconnected() { return disconnected; };
 	virtual bool close() = 0;
-	virtual bool read(std::vector<uint8_t>& bytes, size_t limit = 0);
+	bool read(std::vector<uint8_t>& bytes, size_t limit = 0);
-	virtual bool readWait(std::vector<uint8_t>& bytes, std::chrono::milliseconds timeout = std::chrono::milliseconds(100), size_t limit = 0);
+	bool readWait(std::vector<uint8_t>& bytes, std::chrono::milliseconds timeout = std::chrono::milliseconds(100), size_t limit = 0);
-	virtual bool write(const std::vector<uint8_t>& bytes);
+	bool write(const std::vector<uint8_t>& bytes);
 	virtual bool isEthernet() const { return false; }
 
 	device_eventhandler_t report;
@@ -45,8 +45,15 @@ protected:
 		LAUNCH,
 		WAIT
 	};
+
 	virtual void readTask() = 0;
 	virtual void writeTask() = 0;
+
+	// Overridable in case the driver doesn't want to use writeTask and writeQueue
+	virtual bool writeQueueFull() { return writeQueue.size_approx() > writeQueueSize; }
+	virtual bool writeQueueAlmostFull() { return writeQueue.size_approx() > (writeQueueSize * 3 / 4); }
+	virtual bool writeInternal(const std::vector<uint8_t>& b) { return writeQueue.enqueue(WriteOperation(b)); }
+
 	moodycamel::BlockingConcurrentQueue<uint8_t> readQueue;
 	moodycamel::BlockingConcurrentQueue<WriteOperation> writeQueue;
 	std::thread readThread, writeThread;

include/icsneo/platform/firmio.h

@@ -0,0 +1,13 @@
+#ifndef __FIRMIO_H_
+#define __FIRMIO_H_
+
+#if defined (__linux__)
+#include "icsneo/platform/posix/firmio.h"
+#else
+// This driver is only relevant for communication communication between
+// Linux and CoreMini from the onboard processor of the device, you
+// likely do not want it enabled for your build.
+#warning "This platform is not supported by the firmio driver"
+#endif
+
+#endif

include/icsneo/platform/posix/firmio.h

@@ -0,0 +1,133 @@
+#ifndef __FIRMIO_POSIX_H_
+#define __FIRMIO_POSIX_H_
+
+#ifdef __cplusplus
+
+#include "icsneo/device/neodevice.h"
+#include "icsneo/device/founddevice.h"
+#include "icsneo/communication/driver.h"
+#include "icsneo/api/eventmanager.h"
+#include "icsneo/platform/optional.h"
+#include <string>
+
+namespace icsneo {
+
+// This driver is only relevant for communication communication between
+// Linux and CoreMini from the onboard processor of the device, you
+// likely do not want it enabled for your build.
+class FirmIO : public Driver {
+public:
+	static void Find(std::vector<FoundDevice>& foundDevices);
+
+	FirmIO(device_eventhandler_t err);
+	~FirmIO();
+	bool open() override;
+	bool isOpen() override;
+	bool close() override;
+private:
+	void readTask() override;
+	void writeTask() override;
+	bool writeQueueFull() override;
+	bool writeQueueAlmostFull() override;
+	bool writeInternal(const std::vector<uint8_t>& bytes) override;
+
+	struct DataInfo {
+		uint32_t type;
+		uint32_t offset;
+		uint32_t size;
+	};
+
+	struct ComHeader {
+		uint32_t comVer;
+		struct DataInfo msgqPtrOut;
+		struct DataInfo msgqOut;
+		struct DataInfo shmOut;
+		struct DataInfo msgqPtrIn;
+		struct DataInfo msgqIn;
+		struct DataInfo shmIn;
+	};
+
+	struct Msg {
+		enum class Command : uint32_t {
+			ComData = 0xAA000000,
+			ComFree = 0xAA000001,
+			ComReset = 0xAA000002,
+		};
+		struct Data {
+			uint32_t addr;
+			uint32_t len;
+			uint32_t ref;
+			uint32_t addr1;
+			uint32_t len1;
+			uint32_t ref1;
+			uint32_t reserved;
+		};
+		struct Free {
+			uint32_t refCount;
+			uint32_t ref[6];
+		};
+		union Payload {
+			Data data;
+			Free free;
+		};
+		Command command;
+		Payload payload;
+	};
+
+	class MsgQueue { // mq_t
+	public:
+		bool read(Msg* msg) volatile;
+		bool write(const Msg* msg) volatile;
+		bool isEmpty() const volatile;
+		bool isFull() const volatile;
+
+	private: // These variables are mmaped, don't change their order or add anything
+		uint32_t head;
+		uint32_t tail;
+		uint32_t size;
+		[[maybe_unused]] uint32_t reserved[4];
+		Msg* msgs;
+	};
+
+	class Mempool {
+	public:
+		static constexpr const size_t BlockSize = 4096;
+
+		Mempool(uint8_t* start, uint32_t size, void* virt, uint32_t phys);
+		uint8_t* alloc(uint32_t size);
+		bool free(uint8_t* addr);
+		uint32_t translate(uint8_t* addr) const;
+
+	private:
+		struct BlockInfo {
+			enum class Status : uint32_t {
+				Free = 0,
+				Used = 1,
+			};
+			Status status;
+			uint8_t* addr;
+		};
+
+		uint8_t* const startAddress;
+		const uint32_t totalSize;
+		std::vector<BlockInfo> blocks;
+		std::atomic<uint32_t> usedBlocks;
+
+		void* const virtualAddress;
+		const uint32_t physicalAddress;
+	};
+
+	int fd = 0;
+	uint8_t* vbase = nullptr;
+	volatile ComHeader* header = nullptr;
+	volatile MsgQueue* in = nullptr;
+	std::mutex outMutex;
+	volatile MsgQueue* out = nullptr;
+	optional<Mempool> outMemory;
+};
+
+}
+
+#endif // __cplusplus
+
+#endif // __FIRMIO_POSIX_H_

platform/posix/firmio.cpp

@@ -0,0 +1,328 @@
+#include "icsneo/platform/firmio.h"
+#include "icsneo/communication/network.h"
+#include "icsneo/communication/command.h"
+#include "icsneo/communication/packetizer.h"
+#include "icsneo/communication/decoder.h"
+#include "icsneo/communication/message/serialnumbermessage.h"
+#include <dirent.h>
+#include <cstring>
+#include <iostream>
+#include <sstream>
+#include <fstream>
+#include <map>
+#include <algorithm>
+#include <fcntl.h>
+#include <unistd.h>
+#include <errno.h>
+#include <sys/select.h>
+#include <sys/mman.h>
+
+using namespace icsneo;
+
+#define PHY_ADDR_BASE (0x1E000000) // IPC0
+#define MMAP_LEN (0x1000000)
+#define FIRMIO_DEV "/dev/firmio"
+#define COM_VER (0xC000)
+#define memory_barrier() __sync_synchronize()
+
+void FirmIO::Find(std::vector<FoundDevice>& found) {
+	FirmIO temp([](APIEvent::Type, APIEvent::Severity) {});
+	if(!temp.open())
+		return;
+
+	std::vector<uint8_t> payload = {
+		((1 << 4) | (uint8_t)Network::NetID::Main51), // Packet size of 1 on NETID_MAIN51
+		(uint8_t)Command::RequestSerialNumber
+	};
+	payload.push_back(Packetizer::ICSChecksum(payload));
+	payload.insert(payload.begin(), 0xAA);
+	temp.write(payload);
+
+	Packetizer packetizer([](APIEvent::Type, APIEvent::Severity) {});
+	Decoder decoder([](APIEvent::Type, APIEvent::Severity) {});
+	using namespace std::chrono;
+	const auto start = steady_clock::now();
+	auto timeout = milliseconds(50);
+	while(temp.readWait(payload, timeout)) {
+		timeout -= duration_cast<milliseconds>(steady_clock::now() - start);
+
+		if(!packetizer.input(payload))
+			continue; // A full packet has not yet been read out
+
+		for(const auto& packet : packetizer.output()) {
+			std::shared_ptr<Message> message;
+			if(!decoder.decode(message, packet))
+				continue; // Malformed packet
+
+			const auto serial = std::dynamic_pointer_cast<SerialNumberMessage>(message);
+			if(!serial || serial->deviceSerial.size() != 6)
+				continue; // Not a serial number message
+
+			FoundDevice foundDevice;
+			// Don't need a handle, only one device will be found
+			// Setting one anyway in case anyone checks for 0 as invalid handle
+			foundDevice.handle = 1;
+			memcpy(foundDevice.serial, serial->deviceSerial.c_str(), sizeof(foundDevice.serial) - 1);
+			foundDevice.serial[sizeof(foundDevice.serial) - 1] = '\0';
+
+			foundDevice.makeDriver = [](const device_eventhandler_t& report, neodevice_t&) {
+				return std::unique_ptr<Driver>(new FirmIO(report));
+			};
+
+			found.push_back(foundDevice);
+		}
+	}
+}
+
+FirmIO::~FirmIO() {
+	if(isOpen())
+		close();
+}
+
+bool FirmIO::open() {
+	if(isOpen()) {
+		report(APIEvent::Type::DeviceCurrentlyOpen, APIEvent::Severity::Error);
+		return false;
+	}
+
+	fd = ::open(FIRMIO_DEV, O_RDWR);
+	if(!isOpen()) {
+		//std::cout << "Open of " << ttyPath.c_str() << " failed with " << strerror(errno) << ' ';
+		report(APIEvent::Type::DriverFailedToOpen, APIEvent::Severity::Error);
+		return false;
+	}
+
+	vbase = reinterpret_cast<uint8_t*>(mmap(nullptr, MMAP_LEN, PROT_READ | PROT_WRITE, MAP_SHARED | MAP_LOCKED, fd, PHY_ADDR_BASE));
+	if(vbase == MAP_FAILED) {
+		report(APIEvent::Type::DriverFailedToOpen, APIEvent::Severity::Error);
+		return false;
+	}
+
+	header = reinterpret_cast<ComHeader*>(vbase);
+	if(header->comVer != COM_VER) {
+		report(APIEvent::Type::DriverFailedToOpen, APIEvent::Severity::Error);
+		return false;
+	}
+
+	// Swapping the in and out ptrs here, what the device considers out, we consider in
+	out = reinterpret_cast<MsgQueue*>(header->msgqPtrIn.offset + vbase);
+	in = reinterpret_cast<MsgQueue*>(header->msgqPtrOut.offset + vbase);
+	outMemory.emplace(vbase + header->shmIn.offset, header->shmIn.size, vbase, PHY_ADDR_BASE);
+
+	// Create thread
+	// No thread for writing since we don't need the extra buffer
+	readThread = std::thread(&FirmIO::readTask, this);
+	
+	return true;
+}
+
+bool FirmIO::isOpen() {
+	return fd >= 0; // Negative fd indicates error or not opened yet
+}
+
+bool FirmIO::close() {
+	if(!isOpen() && !isDisconnected()) {
+		report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error);
+		return false;
+	}
+
+	closing = true;
+
+	if(readThread.joinable())
+		readThread.join();
+
+	closing = false;
+	disconnected = false;
+
+	int ret = munmap(vbase, MMAP_LEN);
+	vbase = nullptr;
+
+	ret |= ::close(fd);
+	fd = -1;
+
+	uint8_t flush;
+	WriteOperation flushop;
+	while (readQueue.try_dequeue(flush)) {}
+	while (writeQueue.try_dequeue(flushop)) {}
+
+	if(ret == 0) {
+		return true;
+	} else {
+		report(APIEvent::Type::DriverFailedToClose, APIEvent::Severity::Error);
+		return false;
+	}
+}
+
+void FirmIO::readTask() {
+	EventManager::GetInstance().downgradeErrorsOnCurrentThread();
+	while(!closing && !isDisconnected()) {
+		fd_set rfds = {0};
+		struct timeval tv = {0};
+		FD_SET(fd, &rfds);
+		tv.tv_usec = 50000; // 50ms
+		int ret = ::select(fd + 1, &rfds, NULL, NULL, &tv);
+		if(ret < 0)
+			report(APIEvent::Type::FailedToRead, APIEvent::Severity::Error);
+		if(ret <= 0)
+			continue;
+
+		uint32_t interruptCount = 0;
+		ret = ::read(fd, &interruptCount, sizeof(interruptCount));
+		if(ret < 0)
+			report(APIEvent::Type::FailedToRead, APIEvent::Severity::Error);
+		if(ret < sizeof(interruptCount) || interruptCount < 1)
+			continue;
+
+		std::vector<Msg> toFree;
+		Msg msg;
+		int i = 0;
+		while(!in->isEmpty() && i++ < 100) {
+			if(!in->read(&msg))
+				break;
+
+			switch(msg.command) {
+			case Msg::Command::ComData: {
+				if(toFree.empty() || toFree.back().payload.free.refCount == 7) {
+					toFree.emplace_back();
+					toFree.back().command = Msg::Command::ComFree;
+					toFree.back().payload.free.refCount = 0;
+				}
+
+				// Add this ref to the list of payloads to free
+				// After we process these, we'll send this list back to the device
+				// so that it can free these entries
+				toFree.back().payload.free.ref[toFree.back().payload.free.refCount] = msg.payload.data.ref;
+				toFree.back().payload.free.refCount++;
+
+				// Translate the physical address back to our virtual address space
+				uint8_t* addr = reinterpret_cast<uint8_t*>(msg.payload.data.addr - PHY_ADDR_BASE + vbase);
+				readQueue.enqueue_bulk(addr, msg.payload.data.len);
+				break;
+			}
+			case Msg::Command::ComFree: {
+				std::lock_guard<std::mutex> lk(outMutex);
+				for(uint32_t i = 0; i < msg.payload.free.refCount; i++)
+					outMemory->free(reinterpret_cast<uint8_t*>(msg.payload.free.ref[i]));
+				break;
+			}
+			}
+		}
+
+		while(!toFree.empty()) {
+			std::lock_guard<std::mutex> lk(outMutex);
+			out->write(&toFree.back());
+			toFree.pop_back();
+		}
+	}
+}
+
+void FirmIO::writeTask() {
+	return; // We're overriding Driver::writeInternal() and doing the work there
+}
+
+bool FirmIO::writeQueueFull() {
+	return out->isFull();
+}
+
+bool FirmIO::writeQueueAlmostFull() {
+	// TODO: Better implementation here
+	return writeQueueFull();
+}
+
+bool FirmIO::writeInternal(const std::vector<uint8_t>& bytes) {
+	if(bytes.empty() || bytes.size() > Mempool::BlockSize)
+		return false;
+
+	std::lock_guard<std::mutex> lk(outMutex);
+	uint8_t* sharedData = outMemory->alloc(bytes.size());
+	if(sharedData == nullptr)
+		return false;
+
+	memcpy(sharedData, bytes.data(), bytes.size());
+
+	Msg msg = { Msg::Command::ComData };
+	msg.payload.data.addr = outMemory->translate(sharedData);
+	msg.payload.data.len = static_cast<uint32_t>(bytes.size());
+	msg.payload.data.ref = reinterpret_cast<uint32_t>(sharedData);
+
+	if(!out->write(&msg))
+		return false;
+
+	uint32_t genInterrupt = 0x01;
+	return ::write(fd, &genInterrupt, sizeof(genInterrupt)) == sizeof(genInterrupt);
+}
+
+bool FirmIO::MsgQueue::read(Msg* msg) volatile {
+	if(isEmpty()) // Contains memory_barrier()
+		return false;
+
+	memcpy(msg, &msgs[tail], sizeof(*msg));
+	tail = (tail + 1) & (size - 1);
+	memory_barrier();
+	return true;
+}
+
+bool FirmIO::MsgQueue::write(const Msg* msg) volatile {
+	if(isFull()) // Contains memory_barrier()
+		return false;
+
+	memcpy(&msgs[head], msg, sizeof(*msg));
+	head = (head + 1) & (size - 1);
+	memory_barrier();
+	return true;
+}
+
+bool FirmIO::MsgQueue::isEmpty() const volatile {
+	memory_barrier();
+	return head == tail;
+}
+
+bool FirmIO::MsgQueue::isFull() const volatile {
+	memory_barrier();
+	return ((head + 1) & (size - 1)) == tail;
+}
+
+FirmIO::Mempool::Mempool(uint8_t* start, uint32_t size, void* virt, uint32_t phys)
+	: startAddress(start), totalSize(size), blocks(size / BlockSize), usedBlocks(0),
+	  virtualAddress(virt), physicalAddress(phys) {
+	size_t idx = 0;
+	for(BlockInfo& block : blocks) {
+		block.status = BlockInfo::Status::Free;
+		block.addr = start + idx * BlockSize;
+		idx++;
+	}
+}
+
+uint8_t* FirmIO::Mempool::alloc(uint32_t size) {
+	if(usedBlocks == blocks.size())
+		return nullptr;
+
+	if(size > BlockSize)
+		return nullptr;
+
+	auto found = std::find_if(blocks.begin(), blocks.end(), [](const BlockInfo& b) {
+		return b.status == BlockInfo::Status::Free;
+	});
+	if(found == blocks.end())
+		return nullptr; // No free blocks, inconsistency with usedBlocks
+
+	found->status = BlockInfo::Status::Used;
+	usedBlocks++;
+	return found->addr;
+}
+
+bool FirmIO::Mempool::free(uint8_t* addr) {
+	auto found = std::find_if(blocks.begin(), blocks.end(), [&addr](const BlockInfo& b) {
+		return b.addr == addr;
+	});
+
+	if(found == blocks.end())
+		return false; // Invalid address
+
+	if(found->status != BlockInfo::Status::Used)
+		return false; // Double free
+
+	usedBlocks--;
+	found->status = BlockInfo::Status::Free;
+	return true;
+}