From 63a9a0e611e59bc3716e62cd5aefa49a67ac55e3 Mon Sep 17 00:00:00 2001 From: Lactore Date: Thu, 19 Jan 2023 13:55:25 +0100 Subject: [PATCH] rebase for old pr(#19) and wrap linux socket impl --- src/VRDriver.cpp | 25 +++- src/VRDriver.hpp | 5 +- src/bridge/bridge-unix-sockets.cpp | 162 +++++++++++++++------- src/bridge/bridge-windows-pipes.cpp | 200 +++++++++++++++++----------- src/bridge/bridge.hpp | 68 +++++++++- 5 files changed, 317 insertions(+), 143 deletions(-) diff --git a/src/VRDriver.cpp b/src/VRDriver.cpp index 24b4533..be7eddc 100644 --- a/src/VRDriver.cpp +++ b/src/VRDriver.cpp @@ -6,6 +6,14 @@ #include #include "VRPaths_openvr.hpp" +SlimeVRDriver::VRDriver::VRDriver(): m_pBridge(new Bridge(this)) +{ +} + +SlimeVRDriver::VRDriver::~VRDriver() +{ + delete this->m_pBridge; +} vr::EVRInitError SlimeVRDriver::VRDriver::Init(vr::IVRDriverContext* pDriverContext) { @@ -30,11 +38,15 @@ vr::EVRInitError SlimeVRDriver::VRDriver::Init(vr::IVRDriverContext* pDriverCont Log("SlimeVR Driver Loaded Successfully"); + // Start bridge thread + this->m_pBridge->start(); + return vr::VRInitError_None; } void SlimeVRDriver::VRDriver::Cleanup() { + this->m_pBridge->stop(); } void SlimeVRDriver::VRDriver::RunFrame() @@ -58,12 +70,11 @@ void SlimeVRDriver::VRDriver::RunFrame() // Update devices for(auto& device : this->devices_) device->Update(); - - BridgeStatus status = runBridgeFrame(*this); - if(status == BRIDGE_CONNECTED) { + + if(this->m_pBridge->state() == Bridge::EState::BRIDGE_CONNECTED) { messages::ProtobufMessage* message = google::protobuf::Arena::CreateMessage(&arena); // Read all messages from the bridge - while(getNextBridgeMessage(*message, *this)) { + while(this->m_pBridge->getNextBridgeMessage(*message)) { if(message->has_tracker_added()) { messages::TrackerAdded ta = message->tracker_added(); switch(getDeviceType(static_cast(ta.tracker_role()))) { @@ -95,13 +106,13 @@ void SlimeVRDriver::VRDriver::RunFrame() trackerAdded->set_tracker_role(TrackerRole::HMD); trackerAdded->set_tracker_serial("HMD"); trackerAdded->set_tracker_name("HMD"); - sendBridgeMessage(*message, *this); + this->m_pBridge->sendBridgeMessage(*message); messages::TrackerStatus* trackerStatus = google::protobuf::Arena::CreateMessage(&arena); message->set_allocated_tracker_status(trackerStatus); trackerStatus->set_tracker_id(0); trackerStatus->set_status(messages::TrackerStatus_Status::TrackerStatus_Status_OK); - sendBridgeMessage(*message, *this); + this->m_pBridge->sendBridgeMessage(*message); sentHmdAddMessage = true; Log("Sent HMD hello message"); @@ -166,7 +177,7 @@ void SlimeVRDriver::VRDriver::RunFrame() hmdPosition->set_qz((float) q.z); hmdPosition->set_qw((float) q.w); - sendBridgeMessage(*message, *this); + this->m_pBridge->sendBridgeMessage(*message); } else { // If bridge not connected, assume we need to resend hmd tracker add message sentHmdAddMessage = false; diff --git a/src/VRDriver.hpp b/src/VRDriver.hpp index b8b5a47..636d369 100644 --- a/src/VRDriver.hpp +++ b/src/VRDriver.hpp @@ -13,8 +13,11 @@ #include namespace SlimeVRDriver { + class Bridge; class VRDriver : public IVRDriver { public: + VRDriver(); + ~VRDriver() override; // Inherited via IVRDriver virtual std::vector> GetDevices() override; @@ -35,7 +38,6 @@ namespace SlimeVRDriver { virtual bool ShouldBlockStandbyMode() override; virtual void EnterStandby() override; virtual void LeaveStandby() override; - virtual ~VRDriver() = default; virtual std::optional GetCurrentUniverse() override; @@ -52,6 +54,7 @@ namespace SlimeVRDriver { vr::HmdVector3_t GetPosition(vr::HmdMatrix34_t &matrix); bool sentHmdAddMessage = false; + Bridge *m_pBridge; simdjson::ondemand::parser json_parser; std::optional default_chap_path_ = std::nullopt; diff --git a/src/bridge/bridge-unix-sockets.cpp b/src/bridge/bridge-unix-sockets.cpp index fcd55fc..e06bc78 100644 --- a/src/bridge/bridge-unix-sockets.cpp +++ b/src/bridge/bridge-unix-sockets.cpp @@ -25,6 +25,7 @@ * on unix sockets */ #include "bridge.hpp" +#include "../VRDriver.hpp" #ifdef __linux__ #include "unix-sockets.hpp" #include @@ -73,10 +74,98 @@ BasicLocalClient client{}; inline constexpr int BUFFER_SIZE = 1024; using ByteBuffer = std::array; ByteBuffer byteBuffer; +} + +SlimeVRDriver::Bridge::Bridge(VRDriver *i_pDriver) + : m_pDriver(i_pDriver), m_pPipe(nullptr), m_pBuffer(nullptr), + m_eState(EState::BRIDGE_DISCONNECTED), m_bStop(false) { +} + +SlimeVRDriver::Bridge::~Bridge() { + this->stop(); +} + +void SlimeVRDriver::Bridge::run() { + + messages::ProtobufMessage oMsg; + + while (!this->m_bStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + + switch (this->m_eState) { + case EState::BRIDGE_DISCONNECTED: + attemptPipeConnect(); + break; + case EState::BRIDGE_ERROR: + resetPipe(); + break; + case EState::BRIDGE_CONNECTED: + default: + break; + } + + client.UpdateOnce(); + + // Read all msg + while (this->fetchNextBridgeMessage(oMsg)) { + std::lock_guard lk(this->m_oMutex); + this->m_aRecvQueue.push(std::move(oMsg)); + }; + + + while (true) { + { // Custom scope for send queue mutex + std::lock_guard lk(this->m_oMutex); + if (this->m_aSendQueue.empty()) { + break; + } + + oMsg = std::move(this->m_aSendQueue.front()); + this->m_aSendQueue.pop(); + } + this->sendBridgeMessageFromQueue(oMsg); + } + + } +} +void SlimeVRDriver::Bridge::sendBridgeMessageFromQueue(messages::ProtobufMessage &i_oMessage) { + if (!client.IsOpen()) { + this->m_pDriver->Log("bridge send error: failed to write header"); + this->setBridgeError(); + }; + + const auto bufBegin = byteBuffer.begin(); + const auto bufferSize = static_cast(std::distance(bufBegin, byteBuffer.end())); + const auto msgSize = static_cast(i_oMessage.ByteSizeLong()); + const std::optional msgBeginIt = WriteHeader(bufBegin, bufferSize, msgSize); + if (!msgBeginIt) { + this->m_pDriver->Log("bridge send error: failed to write header"); + return; + } + if (!i_oMessage.SerializeToArray(&(**msgBeginIt), msgSize)) { + this->m_pDriver->Log("bridge send error: failed to serialize"); + return; + } + int bytesToSend = static_cast(std::distance(bufBegin, *msgBeginIt + msgSize)); + if (bytesToSend <= 0) { + this->m_pDriver->Log("bridge send error: empty message"); + return; + } + if (bytesToSend > bufferSize) { + this->m_pDriver->Log("bridge send error: message too big"); + return; + } + try { + client.Send(bufBegin, bytesToSend); + } catch (const std::exception& e) { + this->setBridgeError(); + this->m_pDriver->Log("bridge send error: " + std::string(e.what())); + return; + } } -bool getNextBridgeMessage(messages::ProtobufMessage& message, SlimeVRDriver::VRDriver& driver) { +bool SlimeVRDriver::Bridge::fetchNextBridgeMessage(messages::ProtobufMessage &i_oMessage) { if (!client.IsOpen()) return false; int bytesRecv = 0; @@ -84,7 +173,7 @@ bool getNextBridgeMessage(messages::ProtobufMessage& message, SlimeVRDriver::VRD bytesRecv = client.RecvOnce(byteBuffer.begin(), HEADER_SIZE); } catch (const std::exception& e) { client.Close(); - driver.Log("bridge send error: " + std::string(e.what())); + this->m_pDriver->Log("bridge send error: " + std::string(e.what())); return false; } if (bytesRecv == 0) return false; // no message waiting @@ -92,79 +181,50 @@ bool getNextBridgeMessage(messages::ProtobufMessage& message, SlimeVRDriver::VRD int msgSize = 0; const std::optional msgBeginIt = ReadHeader(byteBuffer.begin(), bytesRecv, msgSize); if (!msgBeginIt) { - driver.Log("bridge recv error: invalid message header or size"); + this->m_pDriver->Log("bridge recv error: invalid message header or size"); return false; } if (msgSize <= 0) { - driver.Log("bridge recv error: empty message"); + this->m_pDriver->Log("bridge recv error: empty message"); return false; } try { if (!client.RecvAll(*msgBeginIt, msgSize)) { - driver.Log("bridge recv error: client closed"); + this->m_pDriver->Log("bridge recv error: client closed"); return false; } } catch (const std::exception& e) { client.Close(); - driver.Log("bridge send error: " + std::string(e.what())); + this->m_pDriver->Log("bridge send error: " + std::string(e.what())); return false; } - if (!message.ParseFromArray(&(**msgBeginIt), msgSize)) { - driver.Log("bridge recv error: failed to parse"); + if (!i_oMessage.ParseFromArray(&(**msgBeginIt), msgSize)) { + this->m_pDriver->Log("bridge recv error: failed to parse"); return false; } return true; } -bool sendBridgeMessage(messages::ProtobufMessage& message, SlimeVRDriver::VRDriver& driver) { - if (!client.IsOpen()) return false; - const auto bufBegin = byteBuffer.begin(); - const auto bufferSize = static_cast(std::distance(bufBegin, byteBuffer.end())); - const auto msgSize = static_cast(message.ByteSizeLong()); - const std::optional msgBeginIt = WriteHeader(bufBegin, bufferSize, msgSize); - if (!msgBeginIt) { - driver.Log("bridge send error: failed to write header"); - return false; - } - if (!message.SerializeToArray(&(**msgBeginIt), msgSize)) { - driver.Log("bridge send error: failed to serialize"); - return false; - } - int bytesToSend = static_cast(std::distance(bufBegin, *msgBeginIt + msgSize)); - if (bytesToSend <= 0) { - driver.Log("bridge send error: empty message"); - return false; - } - if (bytesToSend > bufferSize) { - driver.Log("bridge send error: message too big"); - return false; - } - try { - return client.Send(bufBegin, bytesToSend); - } catch (const std::exception& e) { - client.Close(); - driver.Log("bridge send error: " + std::string(e.what())); - return false; - } +void SlimeVRDriver::Bridge::setBridgeError() { + this->m_eState = EState::BRIDGE_ERROR; } -BridgeStatus runBridgeFrame(SlimeVRDriver::VRDriver& driver) { +void SlimeVRDriver::Bridge::resetPipe() { try { - if (!client.IsOpen()) { - client.Open(SOCKET_PATH); - } - client.UpdateOnce(); - - if (!client.IsOpen()) { - return BRIDGE_DISCONNECTED; - } - return BRIDGE_CONNECTED; + client.Close(); + client.Open(SOCKET_PATH); + this->m_pDriver->Log("Pipe was reset"); } catch (const std::exception& e) { - client.Close(); - driver.Log("bridge error: " + std::string(e.what())); - return BRIDGE_ERROR; + this->m_pDriver->Log("bridge error: " + std::string(e.what())); + setBridgeError(); } + } +void SlimeVRDriver::Bridge::attemptPipeConnect() { + // Open pipe only called the first time + this->resetPipe(); +} + #endif // linux diff --git a/src/bridge/bridge-windows-pipes.cpp b/src/bridge/bridge-windows-pipes.cpp index ef73846..75438a0 100644 --- a/src/bridge/bridge-windows-pipes.cpp +++ b/src/bridge/bridge-windows-pipes.cpp @@ -25,110 +25,150 @@ * on named pipes */ #include "bridge.hpp" +#include "../VRDriver.hpp" + #if defined(WIN32) && defined(BRIDGE_USE_PIPES) + #include #define PIPE_NAME "\\\\.\\pipe\\SlimeVRDriver" -unsigned long lastReconnectFrame = 0; - -HANDLE pipe = INVALID_HANDLE_VALUE; -BridgeStatus currentBridgeStatus = BRIDGE_DISCONNECTED; -char buffer[1024]; - -void updatePipe(SlimeVRDriver::VRDriver &driver); -void resetPipe(SlimeVRDriver::VRDriver &driver); -void attemptPipeConnect(SlimeVRDriver::VRDriver &driver); - -BridgeStatus runBridgeFrame(SlimeVRDriver::VRDriver &driver) { - switch(currentBridgeStatus) { - case BRIDGE_DISCONNECTED: - attemptPipeConnect(driver); - break; - case BRIDGE_ERROR: - resetPipe(driver); - break; - case BRIDGE_CONNECTED: - updatePipe(driver); - break; +SlimeVRDriver::Bridge::Bridge(VRDriver *i_pDriver) + : m_pDriver(i_pDriver), m_pPipe(INVALID_HANDLE_VALUE), m_pBuffer(new char[1024]), + m_eState(EState::BRIDGE_DISCONNECTED), m_bStop(false) {} + +SlimeVRDriver::Bridge::~Bridge() { + this->stop(); + delete[] this->m_pBuffer; +} + +void SlimeVRDriver::Bridge::run() { + + messages::ProtobufMessage oMsg; + + while (!this->m_bStop) { + std::this_thread::sleep_for(std::chrono::milliseconds(3)); + + switch (this->m_eState) { + case EState::BRIDGE_DISCONNECTED: + attemptPipeConnect(); + break; + case EState::BRIDGE_ERROR: + resetPipe(); + break; + case EState::BRIDGE_CONNECTED: + default: + break; + } + + // Read all msg + while (this->fetchNextBridgeMessage(oMsg)) { + std::lock_guard lk(this->m_oMutex); + this->m_aRecvQueue.push(std::move(oMsg)); + }; + + + while (true) { + { // Custom scope for send queue mutex + std::lock_guard lk(this->m_oMutex); + if (this->m_aSendQueue.empty()) { + break; + } + + oMsg = std::move(this->m_aSendQueue.front()); + this->m_aSendQueue.pop(); + } + + this->sendBridgeMessageFromQueue(oMsg); + } + } +} - return currentBridgeStatus; +void SlimeVRDriver::Bridge::start() { + this->m_bStop = false; + this->m_oThread = std::thread(&Bridge::run, this); } -bool getNextBridgeMessage(messages::ProtobufMessage &message, SlimeVRDriver::VRDriver &driver) { +void SlimeVRDriver::Bridge::stop() { + if (this->m_oThread.joinable()) { + this->m_oThread.join(); + } +} + +bool SlimeVRDriver::Bridge::fetchNextBridgeMessage(messages::ProtobufMessage &i_oMessage) { + if (this->m_eState != EState::BRIDGE_CONNECTED) { + return false; + } + DWORD dwRead; DWORD dwAvailable; - if(currentBridgeStatus == BRIDGE_CONNECTED) { - if(PeekNamedPipe(pipe, buffer, 4, &dwRead, &dwAvailable, NULL)) { - if(dwRead == 4) { - uint32_t messageLength = (buffer[3] << 24) | (buffer[2] << 16) | (buffer[1] << 8) | buffer[0]; - if(messageLength > 1024) { - // TODO Buffer overflow - } - if(dwAvailable >= messageLength) { - if(ReadFile(pipe, buffer, messageLength, &dwRead, NULL)) { - if(message.ParseFromArray(buffer + 4, messageLength - 4)) - return true; - } else { - currentBridgeStatus = BRIDGE_ERROR; - driver.Log("Bridge error: " + std::to_string(GetLastError())); - } - } + uint32_t messageLength; + bool bNewMessage = false; + + if (PeekNamedPipe(this->m_pPipe, &messageLength, 4, &dwRead, &dwAvailable, NULL)) { + if (dwRead == 4 && messageLength <= 1023 && dwAvailable >= messageLength) { + if (ReadFile(this->m_pPipe, this->m_pBuffer, messageLength, &dwRead, NULL)) { + bNewMessage = i_oMessage.ParseFromArray(this->m_pBuffer + 4, messageLength - 4); + } else { + setBridgeError(); } - } else { - currentBridgeStatus = BRIDGE_ERROR; - driver.Log("Bridge error: " + std::to_string(GetLastError())); } + } else { + setBridgeError(); } - return false; + + + return bNewMessage; } -bool sendBridgeMessage(messages::ProtobufMessage &message, SlimeVRDriver::VRDriver &driver) { - if(currentBridgeStatus == BRIDGE_CONNECTED) { - uint32_t size = (uint32_t) message.ByteSizeLong(); - if(size > 1020) { - driver.Log("Message too big"); - return false; - } - message.SerializeToArray(buffer + 4, size); - size += 4; - buffer[0] = size & 0xFF; - buffer[1] = (size >> 8) & 0xFF; - buffer[2] = (size >> 16) & 0xFF; - buffer[3] = (size >> 24) & 0xFF; - if(WriteFile(pipe, buffer, size, NULL, NULL)) { - return true; - } - currentBridgeStatus = BRIDGE_ERROR; - driver.Log("Bridge error: " + std::to_string(GetLastError())); +void SlimeVRDriver::Bridge::sendBridgeMessageFromQueue(messages::ProtobufMessage &i_oMessage) { + if (this->m_eState != EState::BRIDGE_CONNECTED) { + return; + } + + uint32_t size = static_cast(i_oMessage.ByteSizeLong()); + if (size > 1020) { + this->m_pDriver->Log("Message too big"); + return; + } + + i_oMessage.SerializeToArray(this->m_pBuffer + 4, size); + size += 4; + *reinterpret_cast(this->m_pBuffer) = size; + if (!WriteFile(this->m_pPipe, this->m_pBuffer, size, NULL, NULL)) { + setBridgeError(); + return; } - return false; } -void updatePipe(SlimeVRDriver::VRDriver &driver) { +void SlimeVRDriver::Bridge::setBridgeError() { + this->m_eState = EState::BRIDGE_ERROR; + this->m_pDriver->Log("Bridge error: " + std::to_string(GetLastError())); } -void resetPipe(SlimeVRDriver::VRDriver &driver) { - if(pipe != INVALID_HANDLE_VALUE) { - CloseHandle(pipe); - pipe = INVALID_HANDLE_VALUE; - currentBridgeStatus = BRIDGE_DISCONNECTED; - driver.Log("Pipe was reset"); +void SlimeVRDriver::Bridge::resetPipe() { + if (this->m_pPipe != INVALID_HANDLE_VALUE) { + CloseHandle(this->m_pPipe); + this->m_pPipe = INVALID_HANDLE_VALUE; + + this->m_eState = EState::BRIDGE_DISCONNECTED; + this->m_pDriver->Log("Pipe was reset"); } } -void attemptPipeConnect(SlimeVRDriver::VRDriver &driver) { - pipe = CreateFileA(PIPE_NAME, - GENERIC_READ | GENERIC_WRITE, - 0, - NULL, - OPEN_EXISTING, - 0, // TODO : Overlapped - NULL); - if(pipe != INVALID_HANDLE_VALUE) { - currentBridgeStatus = BRIDGE_CONNECTED; - driver.Log("Pipe was connected"); +void SlimeVRDriver::Bridge::attemptPipeConnect() { + this->m_pPipe = CreateFileA(PIPE_NAME, + GENERIC_READ | GENERIC_WRITE, + 0, + NULL, + OPEN_EXISTING, + 0, // TODO : Overlapped + NULL); + + if (m_pPipe != INVALID_HANDLE_VALUE) { + this->m_eState = EState::BRIDGE_CONNECTED; + this->m_pDriver->Log("Pipe was connected"); return; } } diff --git a/src/bridge/bridge.hpp b/src/bridge/bridge.hpp index fb1e1bf..7995856 100644 --- a/src/bridge/bridge.hpp +++ b/src/bridge/bridge.hpp @@ -27,10 +27,13 @@ #pragma once #define BRIDGE_USE_PIPES 1 + #include "ProtobufMessages.pb.h" #include #include -#include "../VRDriver.hpp" +#include +#include +#include enum BridgeStatus { BRIDGE_DISCONNECTED = 0, @@ -38,8 +41,65 @@ enum BridgeStatus { BRIDGE_ERROR = 2 }; -BridgeStatus runBridgeFrame(SlimeVRDriver::VRDriver &driver); +namespace SlimeVRDriver { + class VRDriver; + + class Bridge { + public: + enum class EState { + BRIDGE_DISCONNECTED = 0, + BRIDGE_CONNECTED = 1, + BRIDGE_ERROR = 2 + }; + + explicit Bridge(VRDriver *i_pDriver); + + ~Bridge(); + + private: + VRDriver *m_pDriver; + void *m_pPipe; + char *m_pBuffer; + EState m_eState; + std::recursive_mutex m_oMutex; + std::thread m_oThread; + std::queue m_aSendQueue; + std::queue m_aRecvQueue; + bool m_bStop; + private: + void setBridgeError(); + void resetPipe(); + void attemptPipeConnect(); + void run(); + bool fetchNextBridgeMessage(messages::ProtobufMessage &i_oMessage); + void sendBridgeMessageFromQueue(messages::ProtobufMessage &i_oMessage); + + public: + __inline EState state() { + return this->m_eState; + } + + public: + void start(); + + void stop(); + + public: + bool getNextBridgeMessage(messages::ProtobufMessage &i_oMessage) { + std::lock_guard lk(this->m_oMutex); + + if (m_aRecvQueue.empty()) { + return false; + } -bool getNextBridgeMessage(messages::ProtobufMessage &message, SlimeVRDriver::VRDriver &driver); + i_oMessage = std::move(this->m_aRecvQueue.front()); + this->m_aRecvQueue.pop(); + return true; + } -bool sendBridgeMessage(messages::ProtobufMessage &message, SlimeVRDriver::VRDriver &driver); \ No newline at end of file + void sendBridgeMessage(messages::ProtobufMessage &i_oMessage) { + std::lock_guard lk(this->m_oMutex); + this->m_aSendQueue.push(std::move(i_oMessage)); + } + }; +}