From 2dd91325e6189fb8e9cce65fb2646bae6dd5e72e Mon Sep 17 00:00:00 2001 From: Paul Hollinsky Date: Sun, 27 Mar 2022 18:10:24 -0400 Subject: [PATCH] FirmIO: Initial commit --- CMakeLists.txt | 15 ++ communication/driver.cpp | 9 +- device/devicefinder.cpp | 8 + include/icsneo/communication/driver.h | 13 +- include/icsneo/platform/firmio.h | 13 + include/icsneo/platform/posix/firmio.h | 133 ++++++++++ platform/posix/firmio.cpp | 328 +++++++++++++++++++++++++ 7 files changed, 512 insertions(+), 7 deletions(-) create mode 100644 include/icsneo/platform/firmio.h create mode 100644 include/icsneo/platform/posix/firmio.h create mode 100644 platform/posix/firmio.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 2fed4e5..c207b37 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/communication/driver.cpp b/communication/driver.cpp index 64aecd1..3d0bbaf 100644 --- a/communication/driver.cpp +++ b/communication/driver.cpp @@ -45,17 +45,18 @@ bool Driver::write(const std::vector& bytes) { } if(writeBlocks) { - if(writeQueue.size_approx() > writeQueueSize) - while(writeQueue.size_approx() > (writeQueueSize * 3 / 4)) + if(writeQueueFull()) { + 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); diff --git a/device/devicefinder.cpp b/device/devicefinder.cpp index e61297f..f95dfdd 100644 --- a/device/devicefinder.cpp +++ b/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> DeviceFinder::FindAll() { static std::vector driverFoundDevices; driverFoundDevices.clear(); + #ifdef ICSNEO_ENABLE_FIRMIO + FirmIO::Find(driverFoundDevices); + #endif + #ifdef ICSNEO_ENABLE_RAW_ETHERNET PCAP::Find(driverFoundDevices); #endif diff --git a/include/icsneo/communication/driver.h b/include/icsneo/communication/driver.h index fa8bb15..ccdf6b9 100644 --- a/include/icsneo/communication/driver.h +++ b/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& bytes, size_t limit = 0); - virtual bool readWait(std::vector& bytes, std::chrono::milliseconds timeout = std::chrono::milliseconds(100), size_t limit = 0); - virtual bool write(const std::vector& bytes); + bool read(std::vector& bytes, size_t limit = 0); + bool readWait(std::vector& bytes, std::chrono::milliseconds timeout = std::chrono::milliseconds(100), size_t limit = 0); + bool write(const std::vector& 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& b) { return writeQueue.enqueue(WriteOperation(b)); } + moodycamel::BlockingConcurrentQueue readQueue; moodycamel::BlockingConcurrentQueue writeQueue; std::thread readThread, writeThread; diff --git a/include/icsneo/platform/firmio.h b/include/icsneo/platform/firmio.h new file mode 100644 index 0000000..a3a244e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/include/icsneo/platform/posix/firmio.h b/include/icsneo/platform/posix/firmio.h new file mode 100644 index 0000000..d3c32b8 --- /dev/null +++ b/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 + +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& 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& 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 blocks; + std::atomic 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 outMemory; +}; + +} + +#endif // __cplusplus + +#endif // __FIRMIO_POSIX_H_ \ No newline at end of file diff --git a/platform/posix/firmio.cpp b/platform/posix/firmio.cpp new file mode 100644 index 0000000..5a2d07b --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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& found) { + FirmIO temp([](APIEvent::Type, APIEvent::Severity) {}); + if(!temp.open()) + return; + + std::vector 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(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; + if(!decoder.decode(message, packet)) + continue; // Malformed packet + + const auto serial = std::dynamic_pointer_cast(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(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(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(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(header->msgqPtrIn.offset + vbase); + in = reinterpret_cast(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 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(msg.payload.data.addr - PHY_ADDR_BASE + vbase); + readQueue.enqueue_bulk(addr, msg.payload.data.len); + break; + } + case Msg::Command::ComFree: { + std::lock_guard lk(outMutex); + for(uint32_t i = 0; i < msg.payload.free.refCount; i++) + outMemory->free(reinterpret_cast(msg.payload.free.ref[i])); + break; + } + } + } + + while(!toFree.empty()) { + std::lock_guard 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& bytes) { + if(bytes.empty() || bytes.size() > Mempool::BlockSize) + return false; + + std::lock_guard 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(bytes.size()); + msg.payload.data.ref = reinterpret_cast(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; +}