diff --git a/libuavcan/include/uavcan/build_config.hpp b/libuavcan/include/uavcan/build_config.hpp index 5a84f41315..6b8768c3e5 100644 --- a/libuavcan/include/uavcan/build_config.hpp +++ b/libuavcan/include/uavcan/build_config.hpp @@ -213,6 +213,17 @@ static const unsigned FloatComparisonEpsilonMult = UAVCAN_FLOAT_COMPARISON_EPSIL static const unsigned FloatComparisonEpsilonMult = 10; #endif +/** + * Maximum number of CAN acceptance filters available on the platform + */ +#ifdef UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS +/// Explicitly specified by the user. +static const unsigned MaxCanAcceptanceFilters = UAVCAN_MAX_CAN_ACCEPTANCE_FILTERS; +#else +/// Default that should be OK for any platform. +static const unsigned MaxCanAcceptanceFilters = 32; +#endif + /** * This constant defines how many receiver objects will be statically pre-allocated by classes that listen to messages * of type uavcan.protocol.NodeStatus from other nodes. If the number of publishers exceeds this value, extra diff --git a/libuavcan/include/uavcan/driver/can.hpp b/libuavcan/include/uavcan/driver/can.hpp index a15ba2d3f9..64fab68599 100644 --- a/libuavcan/include/uavcan/driver/can.hpp +++ b/libuavcan/include/uavcan/driver/can.hpp @@ -30,16 +30,16 @@ struct UAVCAN_EXPORT CanFrame uint8_t data[MaxDataLen]; uint8_t dlc; ///< Data Length Code - CanFrame() - : id(0) - , dlc(0) + CanFrame() : + id(0), + dlc(0) { fill(data, data + MaxDataLen, uint8_t(0)); } - CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) - : id(can_id) - , dlc((data_len > MaxDataLen) ? MaxDataLen : data_len) + CanFrame(uint32_t can_id, const uint8_t* can_data, uint8_t data_len) : + id(can_id), + dlc((data_len > MaxDataLen) ? MaxDataLen : data_len) { UAVCAN_ASSERT(can_data != NULL); UAVCAN_ASSERT(data_len == dlc); @@ -62,7 +62,9 @@ struct UAVCAN_EXPORT CanFrame StrTight, ///< Minimum string length (default) StrAligned ///< Fixed formatting for any frame }; + std::string toString(StringRepresentation mode = StrTight) const; + #endif /** @@ -83,6 +85,16 @@ struct UAVCAN_EXPORT CanFilterConfig { uint32_t id; uint32_t mask; + + bool operator==(const CanFilterConfig& rhs) const + { + return rhs.id == id && rhs.mask == mask; + } + + CanFilterConfig() : + id(0), + mask(0) + { } }; /** @@ -94,9 +106,9 @@ struct UAVCAN_EXPORT CanSelectMasks uint8_t read; uint8_t write; - CanSelectMasks() - : read(0) - , write(0) + CanSelectMasks() : + read(0), + write(0) { } }; @@ -166,6 +178,11 @@ public: */ virtual ICanIface* getIface(uint8_t iface_index) = 0; + virtual const ICanIface* getIface(uint8_t iface_index) const + { + return const_cast(this)->getIface(iface_index); + } + /** * Total number of available CAN interfaces. * This value shall not change after initialization. diff --git a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp new file mode 100644 index 0000000000..2a0decf265 --- /dev/null +++ b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp @@ -0,0 +1,105 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#ifndef UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED +#define UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +/** + * This class configures hardware acceptance filters (if this feature is present on the particular CAN driver) to + * preclude reception of irrelevant CAN frames on the hardware level. + * + * Configuration starts by creating an object of class @ref CanAcceptanceFilterConfigurator on the stack. + * Through the method configureFilters() it determines the number of available HW filters and the number + * of listeners. In case the number of listeners is higher than the number of available HW filters, the function + * automatically merges configs in the most efficient way until their number is reduced to the number of + * available HW filters. Subsequently obtained configurations are then loaded into the CAN driver. + * + * The maximum number of CAN acceptance filters is predefined in uavcan/build_config.hpp through a constant + * @ref MaxCanAcceptanceFilters. The algorithm doesn't allow to have higher number of HW filters configurations than + * defined by MaxCanAcceptanceFilters. You can change this value according to the number specified in your CAN driver + * datasheet. + */ +class CanAcceptanceFilterConfigurator +{ + /** + * Below constants based on UAVCAN transport layer specification. Masks and ID's depends on message Priority, + * TypeID, TransferID (RequestNotResponse - for service types, BroadcastNotUnicast - for message types). + * For more details refer to uavcan.org/CAN_bus_transport_layer_specification. + * For clarity let's represent "i" as Data Type ID + * DefaultFilterMsgMask = 00111111111110000000000000000 + * DefaultFilterMsgID = 00iiiiiiiiiii0000000000000000, no need to explicitly define, since MsgID initialized as 0. + * DefaultFilterServiceRequestMask = 11111111111100000000000000000 + * DefaultFilterServiceRequestID = 101iiiiiiiii00000000000000000 + * ServiceRespFrameMask = 11100000000000000000000000000 + * ServiceRespFrameID = 10000000000000000000000000000, all Service Response Frames are accepted by HW filters. + */ + static const unsigned DefaultFilterMsgMask = 0x7FF0000; + static const unsigned DefaultFilterServiceRequestID = 0x14000000; + static const unsigned DefaultFilterServiceRequestMask = 0x1FFE0000; + static const unsigned ServiceRespFrameID = 0x10000000; + static const unsigned ServiceRespFrameMask = 0x1C000000; + + typedef uavcan::Multiset MultisetConfigContainer; + + static CanFilterConfig mergeFilters(CanFilterConfig &a_, CanFilterConfig &b_); + static uint8_t countBits(uint32_t n_); + uint16_t getNumFilters() const; + + /** + * Fills the multiset_configs_ to proceed it with computeConfiguration() + */ + int16_t loadInputConfiguration(); + + /** + * This method merges several listeners's filter configurations by predetermined algorithm + * if number of available hardware acceptance filters less than number of listeners + */ + int16_t computeConfiguration(); + + /** + * This method loads the configuration computed with computeConfiguration() to the CAN driver. + */ + int16_t applyConfiguration(); + + INode& node_; //< Node reference is needed for access to ICanDriver and Dispatcher + MultisetConfigContainer multiset_configs_; + +public: + explicit CanAcceptanceFilterConfigurator(INode& node) + : node_(node) + , multiset_configs_(node.getAllocator()) + { } + + /** + * This method invokes loadInputConfiguration(), computeConfiguration() and applyConfiguration() consequently, so that + * optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters() + * @return 0 = success, negative for error. + */ + int configureFilters(); + + /** + * Returns the configuration computed with computeConfiguration(). + * If computeConfiguration() has not been called yet, an empty configuration will be returned. + */ + const MultisetConfigContainer& getConfiguration() const + { + return multiset_configs_; + } +}; + +} + +#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED diff --git a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp new file mode 100644 index 0000000000..16fcb53a48 --- /dev/null +++ b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -0,0 +1,232 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +namespace uavcan +{ +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestID; +const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceRequestMask; +const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameID; +const unsigned CanAcceptanceFilterConfigurator::ServiceRespFrameMask; + +int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration() +{ + multiset_configs_.clear(); + + CanFilterConfig service_resp_cfg; + service_resp_cfg.id = ServiceRespFrameID; + service_resp_cfg.mask = ServiceRespFrameMask; + if (multiset_configs_.emplace(service_resp_cfg) == NULL) + { + return -ErrMemory; + } + + const TransferListenerBase* p = node_.getDispatcher().getListOfMessageListeners().get(); + while (p) + { + CanFilterConfig cfg; + cfg.id = static_cast(p->getDataTypeDescriptor().getID().get()) << 16; + cfg.id |= static_cast(p->getDataTypeDescriptor().getKind()) << 8; + cfg.mask = DefaultFilterMsgMask; + if (multiset_configs_.emplace(cfg) == NULL) + { + return -ErrMemory; + } + p = p->getNextListNode(); + } + + const TransferListenerBase* p1 = node_.getDispatcher().getListOfServiceRequestListeners().get(); + while (p1) + { + CanFilterConfig cfg; + cfg.id = DefaultFilterServiceRequestID; + cfg.id |= static_cast(p1->getDataTypeDescriptor().getID().get()) << 17; + cfg.mask = DefaultFilterServiceRequestMask; + if (multiset_configs_.emplace(cfg) == NULL) + { + return -ErrMemory; + } + p1 = p1->getNextListNode(); + } + + if (multiset_configs_.getSize() == 0) + { + return -ErrLogic; + } + +#if UAVCAN_DEBUG + for (uint16_t i = 0; i < multiset_configs_.getSize(); i++) + { + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::loadInputConfiguration()", "cfg.ID [%u] = %d", i, + multiset_configs_.getByIndex(i)->id); + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::loadInputConfiguration()", "cfg.MK [%u] = %d", i, + multiset_configs_.getByIndex(i)->mask); + } +#endif + + return 0; +} + +int16_t CanAcceptanceFilterConfigurator::computeConfiguration() +{ + const uint16_t acceptance_filters_number = getNumFilters(); + if (acceptance_filters_number == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + UAVCAN_ASSERT(multiset_configs_.getSize() != 0); + + while (acceptance_filters_number < multiset_configs_.getSize()) + { + uint16_t i_rank = 0, j_rank = 0; + uint8_t best_rank = 0; + + const uint16_t multiset_array_size = static_cast(multiset_configs_.getSize()); + + for (uint16_t i_ind = 0; i_ind < multiset_array_size - 1; i_ind++) + { + for (uint16_t j_ind = static_cast(i_ind + 1); j_ind < multiset_array_size; j_ind++) + { + CanFilterConfig temp_config = mergeFilters(*multiset_configs_.getByIndex(i_ind), + *multiset_configs_.getByIndex(j_ind)); + uint8_t rank = countBits(temp_config.mask); + if (rank > best_rank) + { + best_rank = rank; + i_rank = i_ind; + j_rank = j_ind; + } + } + } + + *multiset_configs_.getByIndex(j_rank) = mergeFilters(*multiset_configs_.getByIndex(i_rank), + *multiset_configs_.getByIndex(j_rank)); + multiset_configs_.removeFirst(*multiset_configs_.getByIndex(i_rank)); + } + + UAVCAN_ASSERT(acceptance_filters_number >= multiset_configs_.getSize()); + + return 0; +} + +int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) +{ + CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters]; + const unsigned int filter_array_size = multiset_configs_.getSize(); + + if (filter_array_size > MaxCanAcceptanceFilters) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + for (uint16_t i = 0; i < filter_array_size; i++) + { + CanFilterConfig temp_filter_config = *multiset_configs_.getByIndex(i); + + filter_conf_array[i] = temp_filter_config; + } + + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + ICanIface* iface = can_driver.getIface(i); + if (iface == NULL) + { + return -ErrDriver; + } + int16_t num = iface->configureFilters(reinterpret_cast(&filter_conf_array), + static_cast(filter_array_size)); + if (num < 0) + { + return -ErrDriver; + } + } + + return 0; +} + +int CanAcceptanceFilterConfigurator::configureFilters() +{ + if (getNumFilters() == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + int16_t fill_array_error = loadInputConfiguration(); + if (fill_array_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()"); + return fill_array_error; + } + + int16_t compute_configuration_error = computeConfiguration(); + if (compute_configuration_error != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration"); + return compute_configuration_error; + } + + if (applyConfiguration() != 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); + return -ErrDriver; + } + + return 0; +} + +uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const +{ + static const uint16_t InvalidOut = 0xFFFF; + uint16_t out = InvalidOut; + ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const ICanIface* iface = can_driver.getIface(i); + if (iface == NULL) + { + UAVCAN_ASSERT(0); + out = 0; + break; + } + const uint16_t num = iface->getNumFilters(); + out = min(out, num); + if (out > MaxCanAcceptanceFilters) + { + out = MaxCanAcceptanceFilters; + } + } + + return (out == InvalidOut) ? 0 : out; +} + +CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_) +{ + CanFilterConfig temp_arr; + temp_arr.mask = a_.mask & b_.mask & ~(a_.id ^ b_.id); + temp_arr.id = a_.id & temp_arr.mask; + + return temp_arr; +} + +uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_) +{ + uint8_t c_; // c accumulates the total bits set in v + for (c_ = 0; n_; c_++) + { + n_ &= n_ - 1; // clear the least significant bit set + } + + return c_; +} + +} diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 568af40e89..fd01b82d96 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -14,7 +14,8 @@ #include #include #include "../transport/can/can.hpp" - +#include +#include struct TestNode : public uavcan::INode { diff --git a/libuavcan/test/transport/can/can.hpp b/libuavcan/test/transport/can/can.hpp index 00a038df9e..2d5a781e1d 100644 --- a/libuavcan/test/transport/can/can.hpp +++ b/libuavcan/test/transport/can/can.hpp @@ -150,9 +150,9 @@ public: // cppcheck-suppress unusedFunction // cppcheck-suppress functionConst - virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; } + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return 0; } // cppcheck-suppress unusedFunction - virtual uavcan::uint16_t getNumFilters() const { return 0; } + virtual uavcan::uint16_t getNumFilters() const { return 4; } // decrease number of HW_filters from 9 to 4 virtual uavcan::uint64_t getErrorCount() const { return num_errors; } }; diff --git a/libuavcan/test/transport/can_acceptance_filter_configurator.cpp b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp new file mode 100644 index 0000000000..7418c12f51 --- /dev/null +++ b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp @@ -0,0 +1,157 @@ +/* + * Copyright (C) 2014 Pavel Kirienko , + * Ilia Sheremet + */ + +#include +#include + +#include +#include "../node/test_node.hpp" +#include "uavcan/node/subscriber.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + +template +struct SubscriptionListener +{ + typedef uavcan::ReceivedDataStructure ReceivedDataStructure; + + struct ReceivedDataStructureCopy + { + uavcan::MonotonicTime ts_monotonic; + uavcan::UtcTime ts_utc; + uavcan::TransferType transfer_type; + uavcan::TransferID transfer_id; + uavcan::NodeID src_node_id; + uavcan::uint8_t iface_index; + DataType msg; + + ReceivedDataStructureCopy(const ReceivedDataStructure& s) : + ts_monotonic(s.getMonotonicTimestamp()), + ts_utc(s.getUtcTimestamp()), + transfer_type(s.getTransferType()), + transfer_id(s.getTransferID()), + src_node_id(s.getSrcNodeID()), + iface_index(s.getIfaceIndex()), + msg(s) + { } + }; + + std::vector simple; + std::vector extended; + + void receiveExtended(ReceivedDataStructure& msg) + { + extended.push_back(msg); + } + + void receiveSimple(DataType& msg) + { + simple.push_back(msg); + } + + typedef SubscriptionListener SelfType; + typedef uavcan::MethodBinder ExtendedBinder; + typedef uavcan::MethodBinder SimpleBinder; + + ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); } + SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); } +}; + +static void writeServiceServerCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::protocol::file::BeginFirmwareUpdate::Response& rsp) +{ + std::cout << req << std::endl; + rsp.error = rsp.ERROR_UNKNOWN; +} + +TEST(CanAcceptanceFilter, Basic_test) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + uavcan::DefaultDataTypeRegistrator _reg7; + + SystemClockDriver clock_driver; + CanDriverMock can_driver(1, clock_driver); + TestNode node(can_driver, clock_driver, 24); + + uavcan::Subscriber::ExtendedBinder> sub_1(node); + uavcan::Subscriber::ExtendedBinder> sub_2(node); + uavcan::Subscriber::ExtendedBinder> sub_3(node); + uavcan::Subscriber::ExtendedBinder> sub_4(node); + uavcan::Subscriber::ExtendedBinder> sub_5(node); + uavcan::Subscriber::ExtendedBinder> sub_6(node); + uavcan::Subscriber::ExtendedBinder> sub_6_1(node); + uavcan::ServiceServer server(node); + + SubscriptionListener listener_1; + SubscriptionListener listener_2; + SubscriptionListener listener_3; + SubscriptionListener listener_4; + SubscriptionListener listener_5; + SubscriptionListener listener_6; + + sub_1.start(listener_1.bindExtended()); + sub_2.start(listener_2.bindExtended()); + sub_3.start(listener_3.bindExtended()); + sub_4.start(listener_4.bindExtended()); + sub_5.start(listener_5.bindExtended()); + sub_6.start(listener_6.bindExtended()); + sub_6_1.start(listener_6.bindExtended()); + server.start(writeServiceServerCallback); + std::cout << "Subscribers are initialized ..." << std::endl; + + uavcan::CanAcceptanceFilterConfigurator test_configurator(node); + int configure_filters_assert = test_configurator.configureFilters(); + if (configure_filters_assert == 0) + { + std::cout << "Filters are configured ..." << std::endl; + } + + const auto& configure_array = test_configurator.getConfiguration(); + uint32_t configure_array_size = configure_array.getSize(); + + ASSERT_EQ(configure_filters_assert, 0); + ASSERT_EQ(configure_array_size, 4); + + for (uint16_t i = 0; i