Device: Implement Wireless neoVI Stack

This reverts commit cb47065a47.

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:

communication/packet/wivicommandpacket.cpp

@@ -35,6 +35,28 @@ std::shared_ptr<WiVI::ResponseMessage> 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<const WiVI::CommandPacket::GetAll*>(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<uint8_t> WiVI::CommandPacket::SetSignal::Encode(WiVI::SignalType typ
 
 	return ret;
 }
+
+std::vector<uint8_t> WiVI::CommandPacket::GetAll::Encode() {
+	std::vector<uint8_t> ret(sizeof(WiVI::CommandPacket::GetAll));
+	auto& frame = *reinterpret_cast<WiVI::CommandPacket::GetAll*>(ret.data());
+
+	frame.header.cmd = WiVI::Command::GetAll;
+	frame.header.length = sizeof(frame) - sizeof(frame.header);
+
+	return ret;
+}
+
+std::vector<uint8_t> WiVI::CommandPacket::ClearUploads::Encode(const std::vector<uint8_t>& bitmask) {
+	std::vector<uint8_t> ret(sizeof(WiVI::CommandPacket::ClearUploads) + bitmask.size());
+	auto& frame = *reinterpret_cast<WiVI::CommandPacket::ClearUploads*>(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;
+}

device/device.cpp

@@ -789,13 +789,269 @@ optional<double> Device::getAnalogIO(IO type, size_t number /* = 1 */) {
 	return nullopt;
 }
 
+void Device::wiviThreadBody() {
+	std::shared_ptr<MessageFilter> filter = std::make_shared<MessageFilter>(Message::Type::WiVICommandResponse);
+	std::unique_lock<std::mutex> 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<WiVI::ResponseMessage>(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<uint8_t> 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<WiVI::ResponseMessage>(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lk2(wiviMutex);
+		sleepRequestedCallbacks[idx].first = SleepRequestedCallback();
+		stopWiVIThreadIfNecessary(std::move(lk2));
+	});
+}
+
+optional<bool> 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<MessageFilter> filter = std::make_shared<MessageFilter>(Message::Type::WiVICommandResponse);
+	// Hold this lock so the WiVI stack doesn't issue a WiVICommand at the same time as us
+	std::lock_guard<std::mutex> 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<WiVI::ResponseMessage>(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<MessageFilter> filter = std::make_shared<MessageFilter>(Message::Type::WiVICommandResponse);
+	// Hold this lock so the WiVI stack doesn't issue a WiVICommand at the same time as us
+	std::lock_guard<std::mutex> 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<WiVI::ResponseMessage>(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() {

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,

include/icsneo/communication/packet/wivicommandpacket.h

@@ -122,6 +122,24 @@ struct CommandPacket {
 		WiVI::SignalType type;
 		CoreMiniFixedPointValue value;
 	};
+
+	struct GetAll {
+		static std::vector<uint8_t> Encode();
+
+		Header header;
+		uint8_t version;
+		uint8_t sleepRequest;
+		uint16_t connectionTimeoutMinutes;
+		uint16_t numCaptureInfos;
+		CaptureInfo captureInfos[0];
+	};
+
+	struct ClearUploads {
+		static std::vector<uint8_t> Encode(const std::vector<uint8_t>& bitmask);
+
+		Header header;
+		uint8_t bitmask[0];
+	};
 };
 
 } // namespace WiVI

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<double> 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<bool> 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<std::shared_ptr<FlexRay::Controller>> 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<EthPhyMessage> sendEthPhyMsg(const EthPhyMessage& message, std::chrono::milliseconds timeout = std::chrono::milliseconds(50));
 
 	std::shared_ptr<Communication> com;
@@ -411,7 +457,7 @@ protected:
 	virtual bool afterCommunicationOpen() { return true; }
 
 	virtual bool requiresVehiclePower() const { return true; }
-	
+
 	template<typename Extension>
 	std::shared_ptr<Extension> getExtension() const {
 		std::shared_ptr<Extension> ret;
@@ -473,6 +519,17 @@ private:
 	std::atomic<bool> stopHeartbeatThread{false};
 	std::mutex heartbeatMutex;
 	std::thread heartbeatThread;
+
+	// Wireless neoVI Stack
+	std::atomic<bool> stopWiVIThread{false};
+	std::condition_variable stopWiVIcv;
+	mutable std::mutex wiviMutex;
+	std::thread wiviThread;
+	std::atomic<bool> wiviSleepRequested{false};
+	std::vector<NewCaptureCallback> newCaptureCallbacks;
+	std::vector< std::pair<SleepRequestedCallback, bool /* notified */> > sleepRequestedCallbacks;
+	void wiviThreadBody();
+	void stopWiVIThreadIfNecessary(std::unique_lock<std::mutex> lk);
 };
 
 }

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<Network>& txNetworks) override { setupSupportedRXNetworks(txNetworks); }
+
+	bool supportsWiVI() const override { return true; }
 };
 
 }

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<Network>& txNetworks) override { setupSupportedRXNetworks(txNetworks); }
+
+	bool supportsWiVI() const override { return true; }
 };
 
 }

include/icsneo/device/tree/plasion/plasion.h

@@ -88,6 +88,8 @@ protected:
 		const fire2vnet_status_t* status = reinterpret_cast<const fire2vnet_status_t*>(message->data.data());
 		ethActivationStatus = status->ethernetActivationLineEnabled;
 	}
+
+	bool supportsWiVI() const override { return true; }
 };
 
 }