From a928a1d879608f3d425f937cce9b021e5e8d93ae Mon Sep 17 00:00:00 2001 From: Paul Hollinsky Date: Mon, 2 May 2022 22:34:48 -0400 Subject: [PATCH] Device: Implement Wireless neoVI Stack This reverts commit cb47065a47479d3f06472975d004be36575fd4f6. --- api/icsneocpp/event.cpp | 9 + communication/packet/wivicommandpacket.cpp | 43 +++ device/device.cpp | 260 +++++++++++++++++- include/icsneo/api/event.h | 5 +- .../communication/packet/wivicommandpacket.h | 18 ++ include/icsneo/device/device.h | 59 +++- .../device/tree/neovifire3/neovifire3.h | 2 + .../icsneo/device/tree/neovired2/neovired2.h | 2 + include/icsneo/device/tree/plasion/plasion.h | 2 + 9 files changed, 396 insertions(+), 4 deletions(-) diff --git a/api/icsneocpp/event.cpp b/api/icsneocpp/event.cpp index 4f7d961..3e156a0 100644 --- a/api/icsneocpp/event.cpp +++ b/api/icsneocpp/event.cpp @@ -71,6 +71,7 @@ static constexpr const char* UNSUPPORTED_TX_NETWORK = "Message network is not a static constexpr const char* MESSAGE_MAX_LENGTH_EXCEEDED = "The message was too long."; static constexpr const char* VALUE_NOT_YET_PRESENT = "The value is not yet present."; static constexpr const char* TIMEOUT = "The timeout was reached."; +static constexpr const char* WIVI_NOT_SUPPORTED = "Wireless neoVI functions are not supported on this device."; // Device Errors static constexpr const char* POLLING_MESSAGE_OVERFLOW = "Too many messages have been recieved for the polling message buffer, some have been lost!"; @@ -109,6 +110,8 @@ static constexpr const char* EOF_REACHED = "The requested length exceeds the ava static constexpr const char* SETTINGS_DEFAULTS_USED = "The device settings could not be loaded, the default settings have been applied."; static constexpr const char* ATOMIC_OPERATION_RETRIED = "An operation failed to be atomically completed, but will be retried."; static constexpr const char* ATOMIC_OPERATION_COMPLETED_NONATOMICALLY = "An ideally-atomic operation was completed nonatomically."; +static constexpr const char* WIVI_STACK_REFRESH_FAILED = "The Wireless neoVI stack encountered a communication error."; +static constexpr const char* WIVI_UPLOAD_STACK_OVERFLOW = "The Wireless neoVI upload stack has encountered an overflow condition."; // Transport Errors static constexpr const char* FAILED_TO_READ = "A read operation failed."; @@ -158,6 +161,8 @@ const char* APIEvent::DescriptionForType(Type type) { return VALUE_NOT_YET_PRESENT; case Type::Timeout: return TIMEOUT; + case Type::WiVINotSupported: + return WIVI_NOT_SUPPORTED; // Device Errors case Type::PollingMessageOverflow: @@ -232,6 +237,10 @@ const char* APIEvent::DescriptionForType(Type type) { return ATOMIC_OPERATION_RETRIED; case Type::AtomicOperationCompletedNonatomically: return ATOMIC_OPERATION_COMPLETED_NONATOMICALLY; + case Type::WiVIStackRefreshFailed: + return WIVI_STACK_REFRESH_FAILED; + case Type::WiVIUploadStackOverflow: + return WIVI_UPLOAD_STACK_OVERFLOW; // Transport Errors case Type::FailedToRead: diff --git a/communication/packet/wivicommandpacket.cpp b/communication/packet/wivicommandpacket.cpp index 0fde14b..9aae980 100644 --- a/communication/packet/wivicommandpacket.cpp +++ b/communication/packet/wivicommandpacket.cpp @@ -35,6 +35,28 @@ std::shared_ptr WiVI::CommandPacket::DecodeToMessage(cons msg->value = setSignal.value.ValueInt32; break; } + case WiVI::Command::GetAll: { + if(bytestream.size() < sizeof(WiVI::CommandPacket::GetAll)) + return {}; + + if(bytestream.size() != sizeof(WiVI::CommandPacket::GetAll) + header.length) + return {}; + + const auto& getAll = *reinterpret_cast(bytestream.data()); + msg->responseTo = WiVI::Command::GetAll; + msg->info.emplace(); + msg->info->sleepRequest = getAll.sleepRequest; + msg->info->connectionTimeoutMinutes = getAll.connectionTimeoutMinutes; + + // Check that we have enough data for the capture infos + if(bytestream.size() < sizeof(WiVI::CommandPacket::GetAll) + (sizeof(WiVI::CaptureInfo) * getAll.numCaptureInfos)) + return {}; + + msg->info->captures.resize(getAll.numCaptureInfos); + for(uint16_t i = 0; i < getAll.numCaptureInfos; i++) + msg->info->captures[i] = getAll.captureInfos[i]; + break; + } default: // Unknown command response return {}; } @@ -63,3 +85,24 @@ std::vector WiVI::CommandPacket::SetSignal::Encode(WiVI::SignalType typ return ret; } + +std::vector WiVI::CommandPacket::GetAll::Encode() { + std::vector ret(sizeof(WiVI::CommandPacket::GetAll)); + auto& frame = *reinterpret_cast(ret.data()); + + frame.header.cmd = WiVI::Command::GetAll; + frame.header.length = sizeof(frame) - sizeof(frame.header); + + return ret; +} + +std::vector WiVI::CommandPacket::ClearUploads::Encode(const std::vector& bitmask) { + std::vector ret(sizeof(WiVI::CommandPacket::ClearUploads) + bitmask.size()); + auto& frame = *reinterpret_cast(ret.data()); + + frame.header.cmd = WiVI::Command::ClearUploads; + frame.header.length = uint16_t(ret.size() - sizeof(frame.header)); + memcpy(frame.bitmask, bitmask.data(), bitmask.size()); + + return ret; +} diff --git a/device/device.cpp b/device/device.cpp index 247103a..a5d5653 100644 --- a/device/device.cpp +++ b/device/device.cpp @@ -789,13 +789,269 @@ optional Device::getAnalogIO(IO type, size_t number /* = 1 */) { return nullopt; } +void Device::wiviThreadBody() { + std::shared_ptr filter = std::make_shared(Message::Type::WiVICommandResponse); + std::unique_lock lk(wiviMutex); + + EventManager::GetInstance().downgradeErrorsOnCurrentThread(); + + bool first = true; + while(!stopWiVIThread) { + if(first) // Skip the first wait + first = false; + else + stopWiVIcv.wait_for(lk, std::chrono::seconds(3)); + + // Use the command GetAll to get a WiVI::Info structure from the device + const auto generic = com->waitForMessageSync([this]() { + return com->sendCommand(Command::WiVICommand, WiVI::CommandPacket::GetAll::Encode()); + }, filter); + + if(!generic || generic->type != Message::Type::WiVICommandResponse) { + report(APIEvent::Type::WiVIStackRefreshFailed, APIEvent::Severity::Error); + continue; + } + + const auto resp = std::static_pointer_cast(generic); + if(!resp->success || !resp->info.has_value()) { + report(APIEvent::Type::WiVIStackRefreshFailed, APIEvent::Severity::Error); + continue; + } + + // Now we know we have a WiVI::Info structure + + // Don't process captures unless there is a callback attached, + // we don't want to clear any while nobody's listening. + bool processCaptures = false; + for(const auto& cb : newCaptureCallbacks) { + if(cb) { + processCaptures = true; + break; + } + } + + if(processCaptures) { + std::vector clearMasks; + size_t i = 0; + for(const auto& capture : resp->info->captures) { + i++; + + if(capture.flags.uploadOverflow) + report(APIEvent::Type::WiVIUploadStackOverflow, APIEvent::Severity::Error); + + const auto MaxUploads = sizeof(capture.uploadStack) / sizeof(capture.uploadStack[0]); + auto uploadCount = capture.flags.uploadStackSize + 1u; + if(uploadCount > MaxUploads) { + report(APIEvent::Type::WiVIStackRefreshFailed, APIEvent::Severity::Error); + uploadCount = MaxUploads; + } + + for(size_t j = 0; j < uploadCount; j++) { + const auto& upload = capture.uploadStack[j]; + if(!upload.flags.pending) + continue; // Not complete yet, don't notify + + // Schedule this upload to be cleared from the firmware's stack + if(clearMasks.size() != resp->info->captures.size()) + clearMasks.resize(resp->info->captures.size()); + clearMasks[i] |= (1 << j); + + // Notify the client + for(const auto& cb : newCaptureCallbacks) { + if(cb) { + lk.unlock(); + try { + cb(upload.startSector, upload.endSector); + } catch(...) { + report(APIEvent::Type::Unknown, APIEvent::Severity::Error); + } + lk.lock(); + } + } + } + } + + if(!clearMasks.empty()) { + const auto clearMasksGenericResp = com->waitForMessageSync([this, &clearMasks]() { + return com->sendCommand(Command::WiVICommand, WiVI::CommandPacket::ClearUploads::Encode(clearMasks)); + }, filter); + + if(!clearMasksGenericResp + || clearMasksGenericResp->type != Message::Type::WiVICommandResponse + || !std::static_pointer_cast(clearMasksGenericResp)->success) + report(APIEvent::Type::WiVIStackRefreshFailed, APIEvent::Severity::Error); + } + } + + // Process sleep requests + if(resp->info->sleepRequest & 1 /* sleep requested by VSSAL */) { + // Notify any callers we haven't notified yet + for(auto& cb : sleepRequestedCallbacks) { + if(!cb.second && cb.first) { + cb.second = true; + lk.unlock(); + try { + cb.first(resp->info->connectionTimeoutMinutes); + } catch(...) { + report(APIEvent::Type::Unknown, APIEvent::Severity::Error); + } + lk.lock(); + } + } + } else { + // If the sleepRequest becomes 1 again we will notify again + for(auto& cb : sleepRequestedCallbacks) + cb.second = false; + } + } +} + +void Device::stopWiVIThreadIfNecessary(std::unique_lock lk) { + // The callbacks will be empty std::functions if they are removed + for(const auto& cb : newCaptureCallbacks) { + if(cb) + return; // We still need the WiVI Thread + } + + for(const auto& cb : sleepRequestedCallbacks) { + if(cb.first) + return; // We still need the WiVI Thread + } + + stopWiVIThread = true; + lk.unlock(); + stopWiVIcv.notify_all(); + wiviThread.join(); + wiviThread = std::thread(); +} + +Lifetime Device::addNewCaptureCallback(NewCaptureCallback cb) { + if(!isOpen()) { + report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error); + return {}; + } + + if(!supportsWiVI()) { + report(APIEvent::Type::WiVINotSupported, APIEvent::Severity::Error); + return {}; + } + + std::lock_guard lk(wiviMutex); + if(!wiviThread.joinable()) { + // Start the thread + stopWiVIThread = false; + wiviThread = std::thread([this]() { wiviThreadBody(); }); + } + + size_t idx = 0; + for(; idx < newCaptureCallbacks.size(); idx++) { + if(!newCaptureCallbacks[idx]) // Empty space (previously erased callback) + break; + } + + if(idx == newCaptureCallbacks.size()) // Create a new space + newCaptureCallbacks.push_back(std::move(cb)); + else + newCaptureCallbacks[idx] = std::move(cb); + + // Cleanup function to remove this capture callback + return Lifetime([this, idx]() { + // TODO: Hold a weak ptr to the `this` instead of relying on the user to keep `this` valid + std::unique_lock lk2(wiviMutex); + newCaptureCallbacks[idx] = NewCaptureCallback(); + stopWiVIThreadIfNecessary(std::move(lk2)); + }); +} + +Lifetime Device::addSleepRequestedCallback(SleepRequestedCallback cb) { + if(!isOpen()) { + report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error); + return {}; + } + + if(!supportsWiVI()) { + report(APIEvent::Type::WiVINotSupported, APIEvent::Severity::Error); + return {}; + } + + std::lock_guard lk(wiviMutex); + if(!wiviThread.joinable()) { + // Start the thread + stopWiVIThread = false; + wiviThread = std::thread([this]() { wiviThreadBody(); }); + } + + size_t idx = 0; + for(; idx < sleepRequestedCallbacks.size(); idx++) { + if(!sleepRequestedCallbacks[idx].first) // Empty space (previously erased callback) + break; + } + + if(idx == sleepRequestedCallbacks.size()) // Create a new space + sleepRequestedCallbacks.emplace_back(std::move(cb), false); + else + sleepRequestedCallbacks[idx] = { std::move(cb), false }; + + // Cleanup function to remove this sleep requested callback + return Lifetime([this, idx]() { + // TODO: Hold a weak ptr to the `this` instead of relying on the user to keep `this` valid + std::unique_lock lk2(wiviMutex); + sleepRequestedCallbacks[idx].first = SleepRequestedCallback(); + stopWiVIThreadIfNecessary(std::move(lk2)); + }); +} + +optional Device::isSleepRequested() const { + if(!isOpen()) { + report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error); + return nullopt; + } + + if(!supportsWiVI()) { + report(APIEvent::Type::WiVINotSupported, APIEvent::Severity::Error); + return nullopt; + } + + static std::shared_ptr filter = std::make_shared(Message::Type::WiVICommandResponse); + // Hold this lock so the WiVI stack doesn't issue a WiVICommand at the same time as us + std::lock_guard lk(wiviMutex); + const auto generic = com->waitForMessageSync([this]() { + // VSSAL sets bit0 to indicate that it's waiting to sleep, then + // it waits for Wireless neoVI to acknowledge by clearing it. + // If we set bit1 at the same time we clear bit0, remote wakeup + // will be suppressed (assuming the device supported it in the + // first place) + return com->sendCommand(Command::WiVICommand, WiVI::CommandPacket::GetSignal::Encode(WiVI::SignalType::SleepRequest)); + }, filter); + + if(!generic || generic->type != Message::Type::WiVICommandResponse) { + report(APIEvent::Type::NoDeviceResponse, APIEvent::Severity::Error); + return nullopt; + } + + const auto resp = std::static_pointer_cast(generic); + if(!resp->success || !resp->value.has_value()) { + report(APIEvent::Type::ValueNotYetPresent, APIEvent::Severity::Error); + return nullopt; + } + + return *resp->value; +} + bool Device::allowSleep(bool remoteWakeup) { if(!isOpen()) { report(APIEvent::Type::DeviceCurrentlyClosed, APIEvent::Severity::Error); return false; } + if(!supportsWiVI()) { + report(APIEvent::Type::WiVINotSupported, APIEvent::Severity::Error); + return false; + } + static std::shared_ptr filter = std::make_shared(Message::Type::WiVICommandResponse); + // Hold this lock so the WiVI stack doesn't issue a WiVICommand at the same time as us + std::lock_guard lk(wiviMutex); const auto generic = com->waitForMessageSync([this, remoteWakeup]() { // VSSAL sets bit0 to indicate that it's waiting to sleep, then // it waits for Wireless neoVI to acknowledge by clearing it. @@ -813,12 +1069,12 @@ bool Device::allowSleep(bool remoteWakeup) { } const auto resp = std::static_pointer_cast(generic); - if(!resp->success || !resp->value.has_value()) { + if(!resp->success) { report(APIEvent::Type::ValueNotYetPresent, APIEvent::Severity::Error); return false; } - return *resp->value; + return true; } Lifetime Device::suppressDisconnects() { diff --git a/include/icsneo/api/event.h b/include/icsneo/api/event.h index 9730b9e..054e89f 100644 --- a/include/icsneo/api/event.h +++ b/include/icsneo/api/event.h @@ -48,7 +48,8 @@ public: MessageMaxLengthExceeded = 0x1012, ValueNotYetPresent = 0x1013, Timeout = 0x1014, - + WiVINotSupported = 0x1015, + // Device Events PollingMessageOverflow = 0x2000, NoSerialNumber = 0x2001, // api @@ -86,6 +87,8 @@ public: SettingsDefaultsUsed = 0x2033, AtomicOperationRetried = 0x2034, AtomicOperationCompletedNonatomically = 0x2035, + WiVIStackRefreshFailed = 0x2036, + WiVIUploadStackOverflow = 0x2037, // Transport Events FailedToRead = 0x3000, diff --git a/include/icsneo/communication/packet/wivicommandpacket.h b/include/icsneo/communication/packet/wivicommandpacket.h index fd9e4fc..c14080f 100644 --- a/include/icsneo/communication/packet/wivicommandpacket.h +++ b/include/icsneo/communication/packet/wivicommandpacket.h @@ -122,6 +122,24 @@ struct CommandPacket { WiVI::SignalType type; CoreMiniFixedPointValue value; }; + + struct GetAll { + static std::vector Encode(); + + Header header; + uint8_t version; + uint8_t sleepRequest; + uint16_t connectionTimeoutMinutes; + uint16_t numCaptureInfos; + CaptureInfo captureInfos[0]; + }; + + struct ClearUploads { + static std::vector Encode(const std::vector& bitmask); + + Header header; + uint8_t bitmask[0]; + }; }; } // namespace WiVI diff --git a/include/icsneo/device/device.h b/include/icsneo/device/device.h index 4fcdab1..efdd120 100644 --- a/include/icsneo/device/device.h +++ b/include/icsneo/device/device.h @@ -27,6 +27,7 @@ #include "icsneo/communication/decoder.h" #include "icsneo/communication/io.h" #include "icsneo/communication/message/resetstatusmessage.h" +#include "icsneo/communication/message/wiviresponsemessage.h" #include "icsneo/device/extensions/flexray/controller.h" #include "icsneo/communication/message/flexray/control/flexraycontrolmessage.h" #include "icsneo/communication/message/ethphymessage.h" @@ -289,6 +290,46 @@ public: */ optional getAnalogIO(IO type, size_t number = 1); + typedef std::function< void(uint32_t startSector, uint32_t endSector) > NewCaptureCallback; + + /** + * Add a callback which will be called for all new captures. + * + * This is invalid for devices which are not running the Wireless neoVI stack. + */ + NODISCARD("If the Lifetime is not held, the callback will be immediately removed") + Lifetime addNewCaptureCallback(NewCaptureCallback cb); + + typedef std::function< void(uint16_t connectionTimeoutMinutes) > SleepRequestedCallback; + + /** + * Add a callback which will be called when a Wireless neoVI device is + * ready for sleep, pending any uploads we might want to complete first. + * + * Call Device::allowSleep() once ready to signal that status to the device. + * + * Check Device::isSleepRequested() to check if the sleep request was interrupted. + * In that case, the sleep requested callbacks will be called again. + * + * This is invalid for devices which are not running the Wireless neoVI stack. + */ + NODISCARD("If the Lifetime is not held, the callback will be immediately removed") + Lifetime addSleepRequestedCallback(SleepRequestedCallback cb); + + /** + * Check whether sleep has been requested by a VSSAL Wireless neoVI script. + */ + optional isSleepRequested() const; + + /** + * Signal to a running VSSAL Wireless neoVI script that we are ready for + * sleep. + * + * If @param remoteWakeup is specified, the modem will be kept running in sleep + * mode, where supported. + * + * This is invalid for devices which are not running the Wireless neoVI stack. + */ bool allowSleep(bool remoteWakeup = false); virtual std::vector> getFlexRayControllers() const { return {}; } @@ -319,6 +360,11 @@ public: */ virtual bool getEthPhyRegControlSupported() const { return false; } + /** + * Returns true if this device supports the Wireless neoVI featureset + */ + virtual bool supportsWiVI() const { return false; } + optional sendEthPhyMsg(const EthPhyMessage& message, std::chrono::milliseconds timeout = std::chrono::milliseconds(50)); std::shared_ptr com; @@ -411,7 +457,7 @@ protected: virtual bool afterCommunicationOpen() { return true; } virtual bool requiresVehiclePower() const { return true; } - + template std::shared_ptr getExtension() const { std::shared_ptr ret; @@ -473,6 +519,17 @@ private: std::atomic stopHeartbeatThread{false}; std::mutex heartbeatMutex; std::thread heartbeatThread; + + // Wireless neoVI Stack + std::atomic stopWiVIThread{false}; + std::condition_variable stopWiVIcv; + mutable std::mutex wiviMutex; + std::thread wiviThread; + std::atomic wiviSleepRequested{false}; + std::vector newCaptureCallbacks; + std::vector< std::pair > sleepRequestedCallbacks; + void wiviThreadBody(); + void stopWiVIThreadIfNecessary(std::unique_lock lk); }; } diff --git a/include/icsneo/device/tree/neovifire3/neovifire3.h b/include/icsneo/device/tree/neovifire3/neovifire3.h index 3625287..dc327c8 100644 --- a/include/icsneo/device/tree/neovifire3/neovifire3.h +++ b/include/icsneo/device/tree/neovifire3/neovifire3.h @@ -56,6 +56,8 @@ protected: // The supported TX networks are the same as the supported RX networks for this device void setupSupportedTXNetworks(std::vector& txNetworks) override { setupSupportedRXNetworks(txNetworks); } + + bool supportsWiVI() const override { return true; } }; } diff --git a/include/icsneo/device/tree/neovired2/neovired2.h b/include/icsneo/device/tree/neovired2/neovired2.h index f4ca791..d13b096 100644 --- a/include/icsneo/device/tree/neovired2/neovired2.h +++ b/include/icsneo/device/tree/neovired2/neovired2.h @@ -56,6 +56,8 @@ protected: // The supported TX networks are the same as the supported RX networks for this device void setupSupportedTXNetworks(std::vector& txNetworks) override { setupSupportedRXNetworks(txNetworks); } + + bool supportsWiVI() const override { return true; } }; } diff --git a/include/icsneo/device/tree/plasion/plasion.h b/include/icsneo/device/tree/plasion/plasion.h index 5d7c3e8..b629d9b 100644 --- a/include/icsneo/device/tree/plasion/plasion.h +++ b/include/icsneo/device/tree/plasion/plasion.h @@ -88,6 +88,8 @@ protected: const fire2vnet_status_t* status = reinterpret_cast(message->data.data()); ethActivationStatus = status->ethernetActivationLineEnabled; } + + bool supportsWiVI() const override { return true; } }; }