diff --git a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp index e7c7d6e7a1..22b98809e0 100644 --- a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp +++ b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp @@ -1,10 +1,10 @@ /* - * Copyright (C) 2014 Pavel Kirienko , + * Copyright (C) 2015 Pavel Kirienko , * Ilia Sheremet */ -#ifndef UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED -#define UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED +#ifndef UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED +#define UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED #include #include @@ -21,10 +21,14 @@ namespace uavcan * 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. + * By means of computeConfiguration() method the class determines the number of available HW filters and the number + * of listeners. In case if custom configuration required, it is possible to add it through addFilterConfig(). + * Subsequently obtained configurations are then loaded into the CAN driver by calling the applyConfiguration() method. + * If the cumulative number of configurations obtained by computeConfiguration() and addFilterConfig() is higher than + * the number of available HW filters, configurations will be merged automatically in the most efficient way. + * + * Note that if the application adds additional server or subscriber objects after the filters have been configured, + * the configuration procedure will have to be performed again. * * 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 @@ -57,7 +61,7 @@ private: * HW filters. * DefaultAnonMsgMask = 00000 00000000 00000000 11111111 * DefaultAnonMsgID = 00000 00000000 00000000 00000000, by default the config is added to accept all anonymous - * frames. In case there are no anonymous messages, invoke configureFilters(IgnoreAnonymousMessages). + * frames. In case there are no anonymous messages, invoke computeConfiguration(IgnoreAnonymousMessages). */ static const unsigned DefaultFilterMsgMask = 0xFFFF80; static const unsigned DefaultFilterServiceMask = 0x7F80; @@ -67,12 +71,12 @@ private: typedef uavcan::Multiset MultisetConfigContainer; - static CanFilterConfig mergeFilters(CanFilterConfig &a_, CanFilterConfig &b_); + 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() + * Fills the multiset_configs_ to proceed it with mergeConfigurations() */ int16_t loadInputConfiguration(AnonymousMessages load_mode); @@ -80,35 +84,56 @@ private: * 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(); + int16_t mergeConfigurations(); INode& node_; //< Node reference is needed for access to ICanDriver and Dispatcher MultisetConfigContainer multiset_configs_; + uint16_t filters_number_; public: - explicit CanAcceptanceFilterConfigurator(INode& node) + /** + * @param node Libuavcan node whose subscribers/servers/etc will be used to configure the filters. + * + * @param filters_number Allows to override the maximum number of hardware filters to use. + * If set to zero (which is default), the class will obtain the number of available + * filters from the CAN driver via @ref ICanIface::getNumFilters(). + */ + explicit CanAcceptanceFilterConfigurator(INode& node, uint16_t filters_number = 0) : node_(node) , multiset_configs_(node.getAllocator()) + , filters_number_(filters_number) { } /** - * This method invokes loadInputConfiguration(), computeConfiguration() and applyConfiguration() consequently, so - * that optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters() + * This method invokes loadInputConfiguration() and mergeConfigurations() consequently + * in order to comute optimal filter configurations for the current hardware. + * + * It can only be invoked when all of the subscriber and server objects are initialized. + * If new subscriber or server objects are added later, the filters will have to be reconfigured again. * * @param mode Either: AcceptAnonymousMessages - the filters will accept all anonymous messages (this is default) * IgnoreAnonymousMessages - anonymous messages will be ignored * @return 0 = success, negative for error. */ - int configureFilters(AnonymousMessages mode = AcceptAnonymousMessages); + int computeConfiguration(AnonymousMessages mode = AcceptAnonymousMessages); /** - * Returns the configuration computed with computeConfiguration(). - * If computeConfiguration() has not been called yet, an empty configuration will be returned. + * Add an additional filter configuration. + * This method must not be invoked after @ref computeConfiguration(). + * @return 0 = success, negative for error. + */ + int addFilterConfig(const CanFilterConfig& config); + + /** + * This method loads the configuration computed with mergeConfigurations() or explicitly added by addFilterConfig() + * to the CAN driver. Must be called after computeConfiguration() and addFilterConfig(). + * @return 0 = success, negative for error. + */ + int applyConfiguration(); + + /** + * Returns the configuration computed with mergeConfigurations() or added by addFilterConfig(). + * If mergeConfigurations() or addFilterConfig() have not been called yet, an empty configuration will be returned. */ const MultisetConfigContainer& getConfiguration() const { @@ -116,6 +141,31 @@ public: } }; +/** + * This function is a shortcut for @ref CanAcceptanceFilterConfigurator. + * It allows to compute filter configuration and then apply it in just one step. + * It implements only the most common use case; if you have special requirements, + * use @ref CanAcceptanceFilterConfigurator directly. + * + * @param node Refer to @ref CanAcceptanceFilterConfigurator constructor for explanation. + * @param mode Refer to @ref CanAcceptanceFilterConfigurator::computeConfiguration() for explanation. + * @return non-negative on success, negative error code on error. + */ +static inline int configureCanAcceptanceFilters(INode& node, + CanAcceptanceFilterConfigurator::AnonymousMessages mode + = CanAcceptanceFilterConfigurator::AcceptAnonymousMessages) +{ + CanAcceptanceFilterConfigurator cfger(node); + + const int compute_res = cfger.computeConfiguration(mode); + if (compute_res < 0) + { + return compute_res; + } + + return cfger.applyConfiguration(); } -#endif // UAVCAN_BUILD_CONFIG_HPP_INCLUDED +} + +#endif diff --git a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp index 820c0b22d2..a7d5a9f29f 100644 --- a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp +++ b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2014 Pavel Kirienko , + * Copyright (C) 2015 Pavel Kirienko , * Ilia Sheremet */ @@ -21,19 +21,19 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessage if (load_mode == AcceptAnonymousMessages) { CanFilterConfig anon_frame_cfg; - anon_frame_cfg.id = DefaultAnonMsgID; - anon_frame_cfg.mask = DefaultAnonMsgMask; + anon_frame_cfg.id = DefaultAnonMsgID | CanFrame::FlagEFF; + anon_frame_cfg.mask = DefaultAnonMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; if (multiset_configs_.emplace(anon_frame_cfg) == NULL) { return -ErrMemory; } } - CanFilterConfig service_resp_cfg; - service_resp_cfg.id = DefaultFilterServiceID; - service_resp_cfg.id |= static_cast(node_.getNodeID().get()) << 8; - service_resp_cfg.mask = DefaultFilterServiceMask; - if (multiset_configs_.emplace(service_resp_cfg) == NULL) + CanFilterConfig service_cfg; + service_cfg.id = DefaultFilterServiceID; + service_cfg.id |= (static_cast(node_.getNodeID().get()) << 8) | CanFrame::FlagEFF; + service_cfg.mask = DefaultFilterServiceMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; + if (multiset_configs_.emplace(service_cfg) == NULL) { return -ErrMemory; } @@ -42,8 +42,8 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessage while (p != NULL) { CanFilterConfig cfg; - cfg.id = static_cast(p->getDataTypeDescriptor().getID().get()) << 8; - cfg.mask = DefaultFilterMsgMask; + cfg.id = (static_cast(p->getDataTypeDescriptor().getID().get()) << 8) | CanFrame::FlagEFF; + cfg.mask = DefaultFilterMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR; if (multiset_configs_.emplace(cfg) == NULL) { return -ErrMemory; @@ -56,20 +56,10 @@ int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessage 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() +int16_t CanAcceptanceFilterConfigurator::mergeConfigurations() { const uint16_t acceptance_filters_number = getNumFilters(); if (acceptance_filters_number == 0) @@ -112,14 +102,29 @@ int16_t CanAcceptanceFilterConfigurator::computeConfiguration() return 0; } -int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) +int CanAcceptanceFilterConfigurator::applyConfiguration(void) { CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters]; - const unsigned int filter_array_size = multiset_configs_.getSize(); + unsigned int filter_array_size = multiset_configs_.getSize(); + + const uint16_t acceptance_filters_number = getNumFilters(); + if (acceptance_filters_number == 0) + { + UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available"); + return -ErrDriver; + } + + if (filter_array_size > acceptance_filters_number) + { + UAVCAN_TRACE("CanAcceptanceFilter", "Too many filter configurations. Executing computeConfiguration()"); + mergeConfigurations(); + filter_array_size = multiset_configs_.getSize(); + } if (filter_array_size > MaxCanAcceptanceFilters) { UAVCAN_ASSERT(0); + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); return -ErrLogic; } @@ -130,18 +135,30 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) filter_conf_array[i] = temp_filter_config; } +#if UAVCAN_DEBUG + for (uint16_t i = 0; i < multiset_configs_.getSize(); i++) + { + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %u", i, + multiset_configs_.getByIndex(i)->id); + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %u", i, + multiset_configs_.getByIndex(i)->mask); + } +#endif + 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) { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); return -ErrDriver; } int16_t num = iface->configureFilters(reinterpret_cast(&filter_conf_array), static_cast(filter_array_size)); if (num < 0) { + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); return -ErrDriver; } } @@ -149,7 +166,7 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) return 0; } -int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) +int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode) { if (getNumFilters() == 0) { @@ -164,17 +181,11 @@ int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) return fill_array_error; } - int16_t compute_configuration_error = computeConfiguration(); - if (compute_configuration_error != 0) + int16_t merge_configurations_error = mergeConfigurations(); + if (merge_configurations_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 merge_configurations_error; } return 0; @@ -182,28 +193,45 @@ int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) 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++) + if (filters_number_ == 0) { - const ICanIface* iface = can_driver.getIface(i); - if (iface == NULL) + 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++) { - UAVCAN_ASSERT(0); - out = 0; - break; - } - const uint16_t num = iface->getNumFilters(); - out = min(out, num); - if (out > MaxCanAcceptanceFilters) - { - out = MaxCanAcceptanceFilters; + 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; + } + else + { + return filters_number_; + } +} + +int CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config) +{ + if (multiset_configs_.emplace(config) == NULL) + { + return -ErrMemory; } - return (out == InvalidOut) ? 0 : out; + return 0; } CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_) @@ -225,5 +253,4 @@ uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_) return c_; } - } diff --git a/libuavcan/test/transport/can_acceptance_filter_configurator.cpp b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp index ff3e647989..1d3f3fc6a7 100644 --- a/libuavcan/test/transport/can_acceptance_filter_configurator.cpp +++ b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp @@ -123,21 +123,41 @@ TEST(CanAcceptanceFilter, Basic_test) sub_6.start(listener_6.bindExtended()); sub_6_1.start(listener_6.bindExtended()); server.start(writeServiceServerCallback); - std::cout << "Subscribers are initialized ..." << std::endl; + std::cout << "Subscribers are initialized." << std::endl; + uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node, 10); - uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node); - int configure_filters_assert = anon_test_configuration.configureFilters(); - if (configure_filters_assert == 0) - { - std::cout << "Filters are configured with anonymous configuration..." << std::endl; - } + int configure_filters_assert = anon_test_configuration.computeConfiguration(); + ASSERT_EQ(configure_filters_assert, 0); + std::cout << "Filters are calculated with anonymous configuration." << std::endl; const auto& configure_array = anon_test_configuration.getConfiguration(); uint32_t configure_array_size = configure_array.getSize(); + std::cout << "Number of configurations after first time computeConfiguration() invoked: " + << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 9); + std::cout << "Adding two additional configurations ... " << std::endl; + uavcan::CanFilterConfig aux_config_1, aux_config_2; + aux_config_1.id = 911; + aux_config_1.mask = 1488; + aux_config_2.id = 999999; + aux_config_2.mask = 849128412; + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1); ASSERT_EQ(configure_filters_assert, 0); - ASSERT_EQ(configure_array_size, 4); + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2); + ASSERT_EQ(configure_filters_assert, 0); + configure_array_size = configure_array.getSize(); + std::cout << "New configuration anon_container size: " << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 11); + + std::cout << "Applying configuration ... " << std::endl; + configure_filters_assert = anon_test_configuration.applyConfiguration(); + ASSERT_EQ(configure_filters_assert, 0); + std::cout << "Filters are configured." << std::endl; + configure_array_size = configure_array.getSize(); + std::cout << "Final configuration anon_container size: " << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 10); for (uint16_t i = 0; i