diff --git a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index 5a39b5c6fc..ca6635451a 100644 --- a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp +++ b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp @@ -5,12 +5,17 @@ #ifndef UAVCAN_PROTOCOL_NETWORK_COMPAT_CHECKER_HPP_INCLUDED #define UAVCAN_PROTOCOL_NETWORK_COMPAT_CHECKER_HPP_INCLUDED +#include +#include #include #include #include +#include #include #include #include +#include +#include namespace uavcan { @@ -58,18 +63,143 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable INode& getNode() { return ns_sub_.getNode(); } const INode& getNode() const { return ns_sub_.getNode(); } - MonotonicDuration getNetworkDiscoveryDelay() const; + MonotonicDuration getNetworkDiscoveryDelay() const + { + // Base duration is constant - NodeStatus max publication period + MonotonicDuration dur = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_PUBLICATION_PERIOD_MS); + // Additional duration depends on the node priority - gets larger with higher Node ID + dur += MonotonicDuration::fromMSec(getNode().getNodeID().get() * 10); + return dur; + } - NodeID findNextUncheckedNode(); + NodeID findNextUncheckedNode() + { + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + if (nid_mask_present_.test(i) && !nid_mask_checked_.test(i)) + { + nid_mask_checked_[i] = true; + return NodeID(i); + } + } + return NodeID(); + } - int waitForCATSResponse(); + int waitForCATSResponse() + { + while (cats_cln_.isPending()) + { + const int res = getNode().spin(MonotonicDuration::fromMSec(10)); + if (res < 0 || !result_.isOk()) + { + return res; + } + } + return 0; + } - void handleNodeStatus(const ReceivedDataStructure& msg); - void handleCATSResponse(ServiceCallResult& resp); + void handleNodeStatus(const ReceivedDataStructure& msg) + { + if (!nid_mask_present_.test(msg.getSrcNodeID().get())) + { + UAVCAN_TRACE("NodeInitializer", "New node nid=%i", int(msg.getSrcNodeID().get())); + nid_mask_present_[msg.getSrcNodeID().get()] = true; + } - int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind); - int checkOneNode(NodeID nid); - int checkNodes(); + if (msg.getSrcNodeID() == getNode().getNodeID()) + { + UAVCAN_TRACE("NodeInitializer", "Node ID collision; nid=%i", int(msg.getSrcNodeID().get())); + result_.conflicting_node = msg.getSrcNodeID(); + } + } + + void handleCATSResponse(ServiceCallResult& resp) + { + last_cats_request_ok_ = resp.isSuccessful(); + if (last_cats_request_ok_) + { + const DataTypeSignature sign = GlobalDataTypeRegistry::instance(). + computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids); + + UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu", + int(resp.server_node_id.get()), static_cast(sign.get()), + static_cast(resp.response.aggregate_signature)); + + if (sign.get() != resp.response.aggregate_signature) + { + result_.conflicting_node = resp.server_node_id; + } + } + } + + int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind) + { + StaticAssert::check(); + StaticAssert::check(); + + UAVCAN_ASSERT(nid.isUnicast()); + UAVCAN_ASSERT(!cats_cln_.isPending()); + + checking_dtkind_ = kind; + protocol::ComputeAggregateTypeSignature::Request request; + request.kind.value = kind; + GlobalDataTypeRegistry::instance().getDataTypeIDMask(kind, request.known_ids); + + int res = cats_cln_.call(nid, request); + if (res < 0) + { + return res; + } + res = waitForCATSResponse(); + if (res < 0) + { + return res; + } + if (!last_cats_request_ok_) + { + return -ErrFailure; + } + return 0; + } + + int checkOneNode(NodeID nid) + { + if (nid == getNode().getNodeID()) + { + result_.conflicting_node = nid; // NodeID collision + return 0; + } + + const int res = checkOneNodeOneDataTypeKind(nid, DataTypeKindMessage); + if (res < 0 || !result_.isOk()) + { + return res; + } + return checkOneNodeOneDataTypeKind(nid, DataTypeKindService); + } + + int checkNodes() + { + (void)nid_mask_checked_.reset(); + result_ = NetworkCompatibilityCheckResult(); + + while (result_.isOk()) + { + const NodeID nid = findNextUncheckedNode(); + if (nid.isValid()) + { + UAVCAN_TRACE("NodeInitializer", "Checking nid=%i", int(nid.get())); + const int res = checkOneNode(nid); + if (res < 0) + { + result_.num_failed_nodes++; + } + UAVCAN_TRACE("NodeInitializer", "Checked nid=%i result=%i", int(nid.get()), res); + } + else { break; } + } + return 0; + } public: explicit NetworkCompatibilityChecker(INode& node) @@ -90,7 +220,42 @@ public: * * Returns negative error code. */ - int execute(); + int execute() + { + int res = 0; + + if (!getNode().getNodeID().isUnicast()) + { + result_.conflicting_node = getNode().getNodeID(); + goto exit; + } + + res = ns_sub_.start(NodeStatusCallback(this, &NetworkCompatibilityChecker::handleNodeStatus)); + if (res < 0) + { + goto exit; + } + + cats_cln_.setCallback(CATSResponseCallback(this, &NetworkCompatibilityChecker::handleCATSResponse)); + res = cats_cln_.init(); + if (res < 0) + { + goto exit; + } + + res = getNode().spin(getNetworkDiscoveryDelay()); + if (res < 0) + { + goto exit; + } + + res = checkNodes(); + + exit: + ns_sub_.stop(); + cats_cln_.cancel(); + return res; + } /** * This method can return a meaningful result only if @ref execute() has been executed successfully once. @@ -101,7 +266,11 @@ public: * Convenience method. Should be called immediately before @ref execute(). * Returns negative error code. */ - static int publishGlobalDiscoveryRequest(INode& node); + static int publishGlobalDiscoveryRequest(INode& node) + { + Publisher pub(node); + return pub.broadcast(protocol::GlobalDiscoveryRequest()); + } }; } diff --git a/libuavcan/src/protocol/uc_network_compat_checker.cpp b/libuavcan/src/protocol/uc_network_compat_checker.cpp deleted file mode 100644 index 6738f723cb..0000000000 --- a/libuavcan/src/protocol/uc_network_compat_checker.cpp +++ /dev/null @@ -1,200 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#if !UAVCAN_TINY - -#include -#include -#include -#include -#include - -namespace uavcan -{ - -MonotonicDuration NetworkCompatibilityChecker::getNetworkDiscoveryDelay() const -{ - // Base duration is constant - NodeStatus max publication period - MonotonicDuration dur = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_PUBLICATION_PERIOD_MS); - // Additional duration depends on the node priority - gets larger with higher Node ID - dur += MonotonicDuration::fromMSec(getNode().getNodeID().get() * 10); - return dur; -} - -NodeID NetworkCompatibilityChecker::findNextUncheckedNode() -{ - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - if (nid_mask_present_.test(i) && !nid_mask_checked_.test(i)) - { - nid_mask_checked_[i] = true; - return NodeID(i); - } - } - return NodeID(); -} - -int NetworkCompatibilityChecker::waitForCATSResponse() -{ - while (cats_cln_.isPending()) - { - const int res = getNode().spin(MonotonicDuration::fromMSec(10)); - if (res < 0 || !result_.isOk()) - { - return res; - } - } - return 0; -} - -void NetworkCompatibilityChecker::handleNodeStatus(const ReceivedDataStructure& msg) -{ - if (!nid_mask_present_.test(msg.getSrcNodeID().get())) - { - UAVCAN_TRACE("NodeInitializer", "New node nid=%i", int(msg.getSrcNodeID().get())); - nid_mask_present_[msg.getSrcNodeID().get()] = true; - } - - if (msg.getSrcNodeID() == getNode().getNodeID()) - { - UAVCAN_TRACE("NodeInitializer", "Node ID collision; nid=%i", int(msg.getSrcNodeID().get())); - result_.conflicting_node = msg.getSrcNodeID(); - } -} - -void NetworkCompatibilityChecker::handleCATSResponse(ServiceCallResult& resp) -{ - last_cats_request_ok_ = resp.isSuccessful(); - if (last_cats_request_ok_) - { - const DataTypeSignature sign = GlobalDataTypeRegistry::instance(). - computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids); - - UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu", - int(resp.server_node_id.get()), static_cast(sign.get()), - static_cast(resp.response.aggregate_signature)); - - if (sign.get() != resp.response.aggregate_signature) - { - result_.conflicting_node = resp.server_node_id; - } - } -} - -int NetworkCompatibilityChecker::checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind) -{ - StaticAssert::check(); - StaticAssert::check(); - - UAVCAN_ASSERT(nid.isUnicast()); - UAVCAN_ASSERT(!cats_cln_.isPending()); - - checking_dtkind_ = kind; - protocol::ComputeAggregateTypeSignature::Request request; - request.kind.value = kind; - GlobalDataTypeRegistry::instance().getDataTypeIDMask(kind, request.known_ids); - - int res = cats_cln_.call(nid, request); - if (res < 0) - { - return res; - } - res = waitForCATSResponse(); - if (res < 0) - { - return res; - } - if (!last_cats_request_ok_) - { - return -ErrFailure; - } - return 0; -} - -int NetworkCompatibilityChecker::checkOneNode(NodeID nid) -{ - if (nid == getNode().getNodeID()) - { - result_.conflicting_node = nid; // NodeID collision - return 0; - } - - const int res = checkOneNodeOneDataTypeKind(nid, DataTypeKindMessage); - if (res < 0 || !result_.isOk()) - { - return res; - } - return checkOneNodeOneDataTypeKind(nid, DataTypeKindService); -} - -int NetworkCompatibilityChecker::checkNodes() -{ - (void)nid_mask_checked_.reset(); - result_ = NetworkCompatibilityCheckResult(); - - while (result_.isOk()) - { - const NodeID nid = findNextUncheckedNode(); - if (nid.isValid()) - { - UAVCAN_TRACE("NodeInitializer", "Checking nid=%i", int(nid.get())); - const int res = checkOneNode(nid); - if (res < 0) - { - result_.num_failed_nodes++; - } - UAVCAN_TRACE("NodeInitializer", "Checked nid=%i result=%i", int(nid.get()), res); - } - else { break; } - } - return 0; -} - -int NetworkCompatibilityChecker::execute() -{ - int res = 0; - - if (!getNode().getNodeID().isUnicast()) - { - result_.conflicting_node = getNode().getNodeID(); - goto exit; - } - - res = ns_sub_.start(NodeStatusCallback(this, &NetworkCompatibilityChecker::handleNodeStatus)); - if (res < 0) - { - goto exit; - } - - cats_cln_.setCallback(CATSResponseCallback(this, &NetworkCompatibilityChecker::handleCATSResponse)); - res = cats_cln_.init(); - if (res < 0) - { - goto exit; - } - - res = getNode().spin(getNetworkDiscoveryDelay()); - if (res < 0) - { - goto exit; - } - - res = checkNodes(); - -exit: - ns_sub_.stop(); - cats_cln_.cancel(); - return res; -} - -int NetworkCompatibilityChecker::publishGlobalDiscoveryRequest(INode& node) -{ - Publisher pub(node); - return pub.broadcast(protocol::GlobalDiscoveryRequest()); -} - -} - -#endif