sven
/
libicsneo
mirror of https://github.com/intrepidcs/libicsneo.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity

FirmIO: Initial commit

v0.3.0-dev
Paul Hollinsky 2022-03-27 18:10:24 -04:00
parent 008a1620c8
commit 2dd91325e6
7 changed files with 512 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
CMakeLists.txt
View File

@ -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()

9
communication/driver.cpp
View File

@ -45,17 +45,18 @@ bool Driver::write(const std::vector<uint8_t>& 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);

8
device/devicefinder.cpp
View File

@ -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

13
include/icsneo/communication/driver.h
View File

@ -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);
virtual 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 read(std::vector<uint8_t>& bytes, size_t limit = 0);
bool readWait(std::vector<uint8_t>& bytes, std::chrono::milliseconds timeout = std::chrono::milliseconds(100), size_t limit = 0);
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;

13
include/icsneo/platform/firmio.h 100644
View File

@ -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

133
include/icsneo/platform/posix/firmio.h 100644
View File

@ -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_

328
platform/posix/firmio.cpp 100644
View File

@ -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;
}