From a2ed997cb4616f5ca4e150c9c5116e6593269a18 Mon Sep 17 00:00:00 2001 From: Ilia Date: Mon, 14 Dec 2015 00:20:35 +0000 Subject: [PATCH 1/3] socketcan HW filters moved to userspace --- .../linux/include/uavcan_linux/socketcan.hpp | 59 +++++++++++++++---- 1 file changed, 48 insertions(+), 11 deletions(-) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index 638e0874ec..f3abdad535 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -148,6 +148,9 @@ class SocketCanIface : public uavcan::ICanIface std::queue rx_queue_; // TODO: Use pool allocator std::unordered_multiset pending_loopback_ids_; // TODO: Use pool allocator + std::vector hw_filters_container_; + bool filters_configured_ = false; + void registerError(SocketCanError e) { errors_[e]++; } void incrementNumFramesInSocketTxQueue() @@ -301,7 +304,17 @@ class SocketCanIface : public uavcan::ICanIface if (accept) { rx.ts_utc += clock_.getPrivateAdjustment(); - rx_queue_.push(rx); + if (filters_configured_) + { + if (checkHWFilters(rx.frame)) + { + rx_queue_.push(rx); + } + } + else + { + rx_queue_.push(rx); + } } } else if (res == 0) @@ -315,6 +328,22 @@ class SocketCanIface : public uavcan::ICanIface } } + /** + * Returns true if a frame accepted by HW filters + */ + bool checkHWFilters(const uavcan::CanFrame frame) + { + uint16_t container_size = hw_filters_container_.size(); + for (uint16_t i = 0; i < container_size; i++) + { + if (((frame.id & hw_filters_container_[i].mask) ^ hw_filters_container_[i].id) == 0) + { + return 1; + } + } + return 0; + } + public: /** * Takes ownership of socket's file descriptor. @@ -404,38 +433,46 @@ public: assert(0); return -1; } - std::vector< ::can_filter> filts(num_configs); + hw_filters_container_.clear(); + hw_filters_container_.resize(num_configs); + for (unsigned i = 0; i < num_configs; i++) { const uavcan::CanFilterConfig& fc = filter_configs[i]; - filts[i].can_id = fc.id & uavcan::CanFrame::MaskExtID; - filts[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID; + hw_filters_container_[i].id = fc.id & uavcan::CanFrame::MaskExtID; + hw_filters_container_[i].mask = fc.mask & uavcan::CanFrame::MaskExtID; if (fc.id & uavcan::CanFrame::FlagEFF) { - filts[i].can_id |= CAN_EFF_FLAG; + hw_filters_container_[i].id |= CAN_EFF_FLAG; } if (fc.id & uavcan::CanFrame::FlagRTR) { - filts[i].can_id |= CAN_RTR_FLAG; + hw_filters_container_[i].id |= CAN_RTR_FLAG; } if (fc.mask & uavcan::CanFrame::FlagEFF) { - filts[i].can_mask |= CAN_EFF_FLAG; + hw_filters_container_[i].mask |= CAN_EFF_FLAG; } if (fc.mask & uavcan::CanFrame::FlagRTR) { - filts[i].can_mask |= CAN_RTR_FLAG; + hw_filters_container_[i].mask |= CAN_RTR_FLAG; } } - int ret = setsockopt(fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filts.data(), sizeof(::can_filter) * num_configs); - return (ret < 0) ? -1 : 0; + + if (! hw_filters_container_.empty()) + { + filters_configured_ = true; + return 0; + } + + return -1; } /** * SocketCAN emulates the CAN filters in software, so the number of filters is virtually unlimited. * This method returns a constant value. */ - std::uint16_t getNumFilters() const override { return 255; } + std::uint16_t getNumFilters() const override { return 8; } /** * Returns total number of errors of each kind detected since the object was created. From 5563dbacff677c88f74f44a805a206b88dc44efe Mon Sep 17 00:00:00 2001 From: Ilia Date: Wed, 16 Dec 2015 00:33:00 +0000 Subject: [PATCH 2/3] addFilterConfig() added. computeConfiguration() separated from applyConfiguration. Other small corrections --- .../can_acceptance_filter_configurator.hpp | 44 +++++---- .../uc_can_acceptance_filter_configurator.cpp | 95 ++++++++++++------- 2 files changed, 88 insertions(+), 51 deletions(-) diff --git a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp index e7c7d6e7a1..d73adda99e 100644 --- a/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp +++ b/libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp @@ -21,10 +21,11 @@ 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. * * 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 +58,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; @@ -72,7 +73,7 @@ private: 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 +81,44 @@ 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) + 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 + * This method invokes loadInputConfiguration() and mergeConfigurations() consequently, so * that optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters() * * @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 the additional filter configuration to multiset_configs_. This method should be invoked only before + * computeConfiguration() member. + */ + int16_t 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(). + */ + int16_t applyConfiguration(); + + /** + * Returns the configuration computed with mergeConfigurations() or added by addFilterConfig(). + * If mergeConfigurations() or addFilterConfig() has not been called yet, an empty configuration will be returned. */ const MultisetConfigContainer& getConfiguration() const { diff --git a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp index 820c0b22d2..448ebe8405 100644 --- a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp +++ b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -16,24 +16,22 @@ const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID; int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessages load_mode) { - multiset_configs_.clear(); - 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 +40,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 +54,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) @@ -115,12 +103,27 @@ int16_t CanAcceptanceFilterConfigurator::computeConfiguration() int16_t 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()"); + computeConfiguration(IgnoreAnonymousMessages); + filter_array_size = multiset_configs_.getSize(); + } if (filter_array_size > MaxCanAcceptanceFilters) { UAVCAN_ASSERT(0); return -ErrLogic; + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); } for (uint16_t i = 0; i < filter_array_size; i++) @@ -130,6 +133,16 @@ 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] = %d", i, + multiset_configs_.getByIndex(i)->id); + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %d", 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++) { @@ -137,19 +150,21 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) if (iface == NULL) { return -ErrDriver; + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); } int16_t num = iface->configureFilters(reinterpret_cast(&filter_conf_array), static_cast(filter_array_size)); if (num < 0) { return -ErrDriver; + UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration"); } } return 0; } -int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) +int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode) { if (getNumFilters() == 0) { @@ -164,17 +179,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,8 +191,10 @@ int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode) uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const { + if (filters_number_ == 0) + { static const uint16_t InvalidOut = 0xFFFF; - uint16_t out = InvalidOut; + uint16_t out = InvalidOut; ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver(); for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) @@ -204,6 +215,22 @@ uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const } return (out == InvalidOut) ? 0 : out; + } + else + { + return filters_number_; + } +} + +int16_t CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config) +{ + CanFilterConfig supl_config = config; + if (multiset_configs_.emplace(supl_config) == NULL) + { + return -ErrMemory; + } + + return 0; } CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_) From 8a5719248f02f56aaef7c18995e14a92f7ec297a Mon Sep 17 00:00:00 2001 From: Ilia Date: Sat, 19 Dec 2015 16:43:29 +0000 Subject: [PATCH 3/3] HW acceptance filters unit test corrected. specificator changed for cfg.MK-ID print --- .../uc_can_acceptance_filter_configurator.cpp | 4 +- .../can_acceptance_filter_configurator.cpp | 109 ++++++++++++------ 2 files changed, 77 insertions(+), 36 deletions(-) diff --git a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp index 448ebe8405..9a2f9af901 100644 --- a/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp +++ b/libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp @@ -136,9 +136,9 @@ int16_t CanAcceptanceFilterConfigurator::applyConfiguration(void) #if UAVCAN_DEBUG for (uint16_t i = 0; i < multiset_configs_.getSize(); i++) { - UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %d", i, + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %u", i, multiset_configs_.getByIndex(i)->id); - UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %d", i, + UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %u", i, multiset_configs_.getByIndex(i)->mask); } #endif diff --git a/libuavcan/test/transport/can_acceptance_filter_configurator.cpp b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp index ff3e647989..c3d2ba963a 100644 --- a/libuavcan/test/transport/can_acceptance_filter_configurator.cpp +++ b/libuavcan/test/transport/can_acceptance_filter_configurator.cpp @@ -123,21 +123,49 @@ 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; - } + uavcan::CanFilterConfig aux_config_1, aux_config_2; + aux_config_1.id = 0; + aux_config_1.mask = 1488; + aux_config_2.id = 24; + aux_config_2.mask = 72; + int configure_filters_assert; + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_1); + ASSERT_EQ(configure_filters_assert, 0); + configure_filters_assert = anon_test_configuration.addFilterConfig(aux_config_2); + ASSERT_EQ(configure_filters_assert, 0); + + 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 configs after first time computeConfiguration() invoked: " + << configure_array_size << std::endl; + ASSERT_EQ(configure_array_size, 10); + std::cout << "Adding two additional configurations ... " << std::endl; + 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, 12); + + 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