Begin work on FlexRay support

2026-08-05 01:18:36 +02:00 · 2019-10-16 16:43:31 -04:00
parent 0607986114
commit 2f9844df92
65 changed files with 1137 additions and 120 deletions
@@ -191,7 +191,9 @@ bool Device::open() {
 	if(!settings->disabled)
 		settings->refresh();

-	internalHandlerCallbackID = com->addMessageCallback(MessageCallback(MessageFilter(Network::Type::Internal), [this](std::shared_ptr<Message> message) {
+	MessageFilter filter;
+	filter.includeInternalInAny = true;
+	internalHandlerCallbackID = com->addMessageCallback(MessageCallback(filter, [this](std::shared_ptr<Message> message) {
 		handleInternalMessage(message);
 	}));

@@ -228,7 +230,7 @@ bool Device::goOnline() {
 	MessageFilter filter(Network::NetID::Reset_Status);
 	filter.includeInternalInAny = true;

-	// wait until communication is enabled or 10 seconds, whichever comes first
+	// Wait until communication is enabled or 5 seconds, whichever comes first
 	while((std::chrono::system_clock::now() - startTime) < std::chrono::seconds(5)) {
 		if(latestResetStatus && latestResetStatus->comEnabled)
 			break;
@@ -331,6 +333,17 @@ Network Device::getNetworkByNumber(Network::Type type, size_t index) const {
 	return Network::NetID::Invalid;
 }

+void Device::addExtension(std::shared_ptr<DeviceExtension>&& extension) {
+	std::lock_guard<std::mutex> lk(extensionsLock);
+	extensions.push_back(extension);
+}
+
+void Device::forEachExtension(std::function<void(const std::shared_ptr<DeviceExtension>&)> fn) {
+	std::lock_guard<std::mutex> lk(extensionsLock);
+	for(const auto& ext : extensions)
+		fn(ext);
+}
+
 void Device::handleInternalMessage(std::shared_ptr<Message> message) {
 	switch(message->network.getNetID()) {
 		case Network::NetID::Reset_Status:
@@ -339,6 +352,9 @@ void Device::handleInternalMessage(std::shared_ptr<Message> message) {
 		default:
 			break; //std::cout << "HandleInternalMessage got a message from " << message->network << " and it was unhandled!" << std::endl;
 	}
+	forEachExtension([&](const std::shared_ptr<DeviceExtension>& ext) {
+		ext->handleMessage(message);
+	});
 }

 void Device::updateLEDState() {
@@ -0,0 +1,120 @@
+#include "icsneo/device/extensions/flexray/controller.h"
+#include "icsneo/device/device.h"
+
+using namespace icsneo;
+
+std::shared_ptr<FlexRayControlMessage> FlexRay::Controller::getStatus() const {
+	std::lock_guard<std::mutex> lk(statusLock);
+	return status;
+}
+
+void FlexRay::Controller::_setStatus(std::shared_ptr<FlexRayControlMessage> msg) {
+	std::lock_guard<std::mutex> lk(statusLock);
+	status = msg;
+}
+
+void FlexRay::Controller::getReady() {
+	
+}
+
+void FlexRay::Controller::start() {
+	if(true) // TODO something
+		getReady();
+	if(wakeupBeforeStart)
+		setCurrentPOCCommand(FlexRay::POCCommand::Wakeup);
+	if(allowColdstart)
+		setCurrentPOCCommand(FlexRay::POCCommand::AllowColdstart);
+	setCurrentPOCCommand(FlexRay::POCCommand::Run);
+}
+
+std::pair<bool, FlexRay::POCCommand> FlexRay::Controller::getCurrentPOCCommand(std::chrono::milliseconds timeout) const {
+	const auto val = readRegister(ERAYRegister::SUCC1, timeout);
+	return {val.first, FlexRay::POCCommand(val.second & 0x0000000F)};
+}
+
+bool FlexRay::Controller::setCurrentPOCCommand(FlexRay::POCCommand cmd, bool checkForSuccess, std::chrono::milliseconds timeout) {
+	const auto beforeWrite = std::chrono::steady_clock::now();
+
+	if(!writeRegister(ERAYRegister::SUCC1, uint32_t(cmd), true, timeout))
+		return false;
+	if(!checkForSuccess)
+		return true;
+	
+	const auto writeDuration = std::chrono::steady_clock::now() - beforeWrite;
+	timeout = std::chrono::duration_cast<std::chrono::milliseconds>(writeDuration - timeout);
+	if(timeout.count() <= 0)
+		return false; // Out of time!
+
+	return wasCommandSuccessful(timeout);
+}
+
+bool FlexRay::Controller::wasCommandSuccessful(std::chrono::milliseconds timeout) const {
+	const auto val = getCurrentPOCCommand(timeout);
+	return val.first && val.second != FlexRay::POCCommand::CommandNotAccepted;
+}
+
+std::pair<bool, uint32_t> FlexRay::Controller::readRegister(ERAYRegister reg, std::chrono::milliseconds timeout) const {
+	std::lock_guard<std::mutex> lk(readRegisterLock);
+	device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildReadCCRegsArgs(index, uint16_t(reg)));
+	std::shared_ptr<FlexRayControlMessage> resp;
+	const auto start = std::chrono::steady_clock::now();
+	while(!resp && (std::chrono::steady_clock::now() - start) < timeout) {
+		auto msg = device.com->waitForMessageSync(MessageFilter(icsneo::Network::NetID::FlexRayControl), timeout);
+		if(auto frmsg = std::dynamic_pointer_cast<FlexRayControlMessage>(msg)) {
+			if(frmsg->decoded && frmsg->controller == index && frmsg->opcode == FlexRay::Opcode::ReadCCRegs)
+				resp = frmsg;
+		}
+	}
+	if(resp)
+		return {true, resp->registers[0]};
+	else
+		return {false, 0};
+}
+
+uint32_t FlexRay::Controller::readRegisterOr(ERAYRegister reg, uint32_t defaultValue, std::chrono::milliseconds timeout) const {
+	auto ret = readRegister(reg, timeout);
+	return ret.first ? ret.second : defaultValue;
+}
+
+bool FlexRay::Controller::writeRegister(
+	ERAYRegister reg,
+	uint32_t value,
+	uint32_t mask,
+	bool waitForPOCReady,
+	std::chrono::milliseconds timeout) {
+
+	if(mask != 0xffffffff) {
+		const auto beforeRead = std::chrono::steady_clock::now();
+		auto pair = readRegister(reg, timeout);
+		if(!pair.first)
+			return false; // Couldn't read, so we don't want to try to write anything
+		auto readDuration = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - beforeRead);
+		timeout = readDuration - timeout;
+		if(timeout.count() <= 0)
+			return false; // Out of time!
+		pair.second &= ~mask;
+		pair.second |= value & mask;
+		value = pair.second;
+	}
+	return writeRegister(reg, value, waitForPOCReady, timeout);
+}
+
+bool FlexRay::Controller::writeRegister(
+	ERAYRegister reg,
+	uint32_t value,
+	bool waitForPOCReady,
+	std::chrono::milliseconds timeout) {
+
+	if(waitForPOCReady) {
+		const auto start = std::chrono::steady_clock::now();
+		bool pocBusy = isPOCBusy();
+		while(pocBusy && (std::chrono::steady_clock::now() - start) < timeout) {
+			pocBusy = isPOCBusy();
+		}
+		if(pocBusy) // timeout
+			return false;
+	}
+
+	device.com->sendCommand(Command::FlexRayControl, FlexRayControlMessage::BuildWriteCCRegArgs(index, uint16_t(reg), value));
+	return true; // Does the device send anything back to tell us this actually happened?
+}
@@ -0,0 +1,31 @@
+#include "icsneo/device/extensions/flexray/extension.h"
+#include "icsneo/device/device.h"
+
+using namespace icsneo;
+
+FlexRay::Extension::Extension(Device& device, uint8_t controllerCount) : DeviceExtension(device) {
+	for(uint8_t i = 0; i < controllerCount; i++)
+		controllers.emplace_back(std::make_shared<FlexRay::Controller>(device, i));
+}
+
+void FlexRay::Extension::handleMessage(const std::shared_ptr<Message>& message) {
+	switch(message->network.getNetID()) {
+		case Network::NetID::FlexRayControl: {
+			auto msg = std::dynamic_pointer_cast<FlexRayControlMessage>(message);
+			if(!msg || !msg->decoded)
+				return;
+			switch(msg->opcode) {
+				case FlexRay::Opcode::ReadCCStatus:
+					if(auto status = std::dynamic_pointer_cast<FlexRayControlMessage>(message)) { // TODO else report error?
+						if(status->controller >= controllers.size())
+							return; // TODO error
+						controllers[status->controller]->_setStatus(status);
+					}
+					break;
+			}
+			break;
+		}
+		default:
+			break;
+	}
+}