Merge pull request #82 from ilia-sheremet/master

Can_acceptance_filter_configurator and unit test are corrected

libuavcan/include/uavcan/transport/can_acceptance_filter_configurator.hpp

@@ -1,10 +1,10 @@
 /*
- * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>,
  *                    Ilia  Sheremet <illia.sheremet@gmail.com>
  */
 
-#ifndef UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
+#ifndef UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
-#define UAVCAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
+#define UAVCAN_TRANSPORT_CAN_ACCEPTANCE_FILTER_CONFIGURATOR_HPP_INCLUDED
 
 #include <cassert>
 #include <uavcan/data_type.hpp>
@@ -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
+ * By means of computeConfiguration() method the class 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
+ * of listeners. In case if custom configuration required, it is possible to add it through addFilterConfig().
- * automatically merges configs in the most efficient way until their number is reduced to the number of
+ * Subsequently obtained configurations are then loaded into the CAN driver by calling the applyConfiguration() method.
- * available HW filters. Subsequently obtained configurations are then loaded into the CAN driver.
+ * 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<CanFilterConfig> 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();
+    int16_t mergeConfigurations();
-
-    /**
-     * 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_;
+    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
+     * This method invokes loadInputConfiguration() and mergeConfigurations() consequently
-     * that optimal acceptance filter configuration will be computed and loaded through CanDriver::configureFilters()
+     * 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().
+     * Add an additional filter configuration.
-     * If computeConfiguration() has not been called yet, an empty configuration will be returned.
+     * 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

libuavcan/src/transport/uc_can_acceptance_filter_configurator.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>,
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>,
  *                    Ilia  Sheremet <illia.sheremet@gmail.com>
  */
 
@@ -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.id = DefaultAnonMsgID | CanFrame::FlagEFF;
-        anon_frame_cfg.mask = DefaultAnonMsgMask;
+        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;
+    CanFilterConfig service_cfg;
-    service_resp_cfg.id = DefaultFilterServiceID;
+    service_cfg.id = DefaultFilterServiceID;
-    service_resp_cfg.id |= static_cast<uint32_t>(node_.getNodeID().get()) << 8;
+    service_cfg.id |= (static_cast<uint32_t>(node_.getNodeID().get()) << 8) | CanFrame::FlagEFF;
-    service_resp_cfg.mask = DefaultFilterServiceMask;
+    service_cfg.mask = DefaultFilterServiceMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR;
-    if (multiset_configs_.emplace(service_resp_cfg) == NULL)
+    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<uint32_t>(p->getDataTypeDescriptor().getID().get()) << 8;
+        cfg.id = (static_cast<uint32_t>(p->getDataTypeDescriptor().getID().get()) << 8) | CanFrame::FlagEFF;
-        cfg.mask = DefaultFilterMsgMask;
+        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<CanFilterConfig*>(&filter_conf_array),
                                               static_cast<uint16_t>(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();
+    int16_t merge_configurations_error = mergeConfigurations();
-    if (compute_configuration_error != 0)
+    if (merge_configurations_error != 0)
     {
         UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration");
-        return compute_configuration_error;
+        return merge_configurations_error;
-    }
-
-    if (applyConfiguration() != 0)
-    {
-        UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
-        return -ErrDriver;
     }
 
     return 0;
@@ -182,28 +193,45 @@ int CanAcceptanceFilterConfigurator::configureFilters(AnonymousMessages mode)
 
 uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const
 {
-    static const uint16_t InvalidOut = 0xFFFF;
+    if (filters_number_ == 0)
-    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);
+        static const uint16_t InvalidOut = 0xFFFF;
-        if (iface == NULL)
+        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);
+            const ICanIface* iface = can_driver.getIface(i);
-            out = 0;
+            if (iface == NULL)
-            break;
+            {
-        }
+                UAVCAN_ASSERT(0);
-        const uint16_t num = iface->getNumFilters();
+                out = 0;
-        out = min(out, num);
+                break;
-        if (out > MaxCanAcceptanceFilters)
+            }
-        {
+            const uint16_t num = iface->getNumFilters();
-            out = MaxCanAcceptanceFilters;
+            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<const CanFilterConfig&>(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_;
 }
-
 }

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.computeConfiguration();
-    int configure_filters_assert = anon_test_configuration.configureFilters();
+    ASSERT_EQ(configure_filters_assert, 0);
-    if (configure_filters_assert == 0)
+    std::cout << "Filters are calculated with anonymous configuration." << std::endl;
-    {
-        std::cout << "Filters are configured 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<configure_array_size; i++)
     {
@@ -145,42 +165,55 @@ TEST(CanAcceptanceFilter, Basic_test)
         std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl;
     }
 
-    ASSERT_EQ(configure_array.getByIndex(0)->id, 256000);
+    ASSERT_EQ(configure_array.getByIndex(0)->id, 911);
-    ASSERT_EQ(configure_array.getByIndex(0)->mask, 16771968);
+    ASSERT_EQ(configure_array.getByIndex(0)->mask, 1488);
-    ASSERT_EQ(configure_array.getByIndex(1)->id, 0);
+    ASSERT_EQ(configure_array.getByIndex(1)->id, 999999);
-    ASSERT_EQ(configure_array.getByIndex(1)->mask, 255);
+    ASSERT_EQ(configure_array.getByIndex(1)->mask, 849128412);
-    ASSERT_EQ(configure_array.getByIndex(2)->id, 6272);
+    ASSERT_EQ(configure_array.getByIndex(2)->id, 2147746048);
-    ASSERT_EQ(configure_array.getByIndex(2)->mask, 32640);
+    ASSERT_EQ(configure_array.getByIndex(2)->mask, 3774873472);
-    ASSERT_EQ(configure_array.getByIndex(3)->id, 262144);
+    ASSERT_EQ(configure_array.getByIndex(3)->id, 2147744768);
-    ASSERT_EQ(configure_array.getByIndex(3)->mask, 16771200);
+    ASSERT_EQ(configure_array.getByIndex(3)->mask, 3774873472);
+    ASSERT_EQ(configure_array.getByIndex(4)->id, 2147739648);
+    ASSERT_EQ(configure_array.getByIndex(4)->mask, 3774873472);
+    ASSERT_EQ(configure_array.getByIndex(5)->id, 2147746816);
+    ASSERT_EQ(configure_array.getByIndex(5)->mask, 3774873472);
+    ASSERT_EQ(configure_array.getByIndex(6)->id, 2147746304);
+    ASSERT_EQ(configure_array.getByIndex(6)->mask, 3774873472);
+    ASSERT_EQ(configure_array.getByIndex(7)->id, 2147483648);
+    ASSERT_EQ(configure_array.getByIndex(7)->mask, 3758096639);
+    ASSERT_EQ(configure_array.getByIndex(8)->id, 2147489920);
+    ASSERT_EQ(configure_array.getByIndex(8)->mask, 3758129024);
+    ASSERT_EQ(configure_array.getByIndex(9)->id, 2147749888);
+    ASSERT_EQ(configure_array.getByIndex(9)->mask, 3774873472);
 
-
+    uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node, 4);
-    uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node);
+    configure_filters_assert = no_anon_test_confiruration.computeConfiguration
-    configure_filters_assert = no_anon_test_confiruration.configureFilters
                                    (uavcan::CanAcceptanceFilterConfigurator::IgnoreAnonymousMessages);
-    if (configure_filters_assert == 0)
+    ASSERT_EQ(configure_filters_assert, 0);
-    {
+    std::cout << "Filters are configured without anonymous configuration." << std::endl;
-        std::cout << "Filters are configured without anonymous configuration..." << std::endl;
+
-    }
+    configure_filters_assert = no_anon_test_confiruration.applyConfiguration();
+    ASSERT_EQ(configure_filters_assert, 0);
+    std::cout << "Filters are configured without anonymous configuration." << std::endl;
+
     const auto& configure_array_2 = no_anon_test_confiruration.getConfiguration();
     configure_array_size = configure_array_2.getSize();
-
     ASSERT_EQ(configure_filters_assert, 0);
     ASSERT_EQ(configure_array_size, 4);
 
     for (uint16_t i = 0; i<configure_array_size; i++)
     {
-        std::cout << "config.ID [" << i << "]= " << configure_array_2.getByIndex(i)->id << std::endl;
+        std::cout << "config.ID [" << i << "] = " << configure_array_2.getByIndex(i)->id << std::endl;
-        std::cout << "config.MK [" << i << "]= " << configure_array_2.getByIndex(i)->mask << std::endl;
+        std::cout << "config.MK [" << i << "] = " << configure_array_2.getByIndex(i)->mask << std::endl;
     }
 
-    ASSERT_EQ(configure_array_2.getByIndex(0)->id, 256000);
+    ASSERT_EQ(configure_array_2.getByIndex(0)->id, 2147739648);
-    ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 16771968);
+    ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 3774868352);
-    ASSERT_EQ(configure_array_2.getByIndex(1)->id, 262144);
+    ASSERT_EQ(configure_array_2.getByIndex(1)->id, 2147745792);
-    ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 16776320);
+    ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 3774872704);
-    ASSERT_EQ(configure_array_2.getByIndex(2)->id, 6272);
+    ASSERT_EQ(configure_array_2.getByIndex(2)->id, 2147489920);
-    ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 32640);
+    ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 3758129024);
-    ASSERT_EQ(configure_array_2.getByIndex(3)->id, 262144);
+    ASSERT_EQ(configure_array_2.getByIndex(3)->id, 2147745792);
-    ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 16771968);
+    ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 3774868352);
 }
 #endif

libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp

@@ -1,5 +1,6 @@
 /*
- * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Copyright (C) 2014-2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ *                         Ilia Sheremet <illia.sheremet@gmail.com>
  */
 
 #pragma once
@@ -58,7 +59,7 @@ class SocketCanIface : public uavcan::ICanIface
 {
     static inline ::can_frame makeSocketCanFrame(const uavcan::CanFrame& uavcan_frame)
     {
-        ::can_frame sockcan_frame { uavcan_frame.id & uavcan::CanFrame::MaskExtID, uavcan_frame.dlc, { } };
+        ::can_frame sockcan_frame { uavcan_frame.id& uavcan::CanFrame::MaskExtID, uavcan_frame.dlc, { } };
         (void)std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data);
         if (uavcan_frame.isExtended())
         {
@@ -148,6 +149,8 @@ class SocketCanIface : public uavcan::ICanIface
     std::queue<RxItem> rx_queue_;                                   // TODO: Use pool allocator
     std::unordered_multiset<std::uint32_t> pending_loopback_ids_;   // TODO: Use pool allocator
 
+    std::vector<::can_filter> hw_filters_container_;
+
     void registerError(SocketCanError e) { errors_[e]++; }
 
     void incrementNumFramesInSocketTxQueue()
@@ -225,6 +228,16 @@ class SocketCanIface : public uavcan::ICanIface
         {
             return (res < 0 && errno == EWOULDBLOCK) ? 0 : res;
         }
+        /*
+         * Flags
+         */
+        loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
+
+        if (!loopback && !checkHWFilters(sockcan_frame))
+        {
+            return 0;
+        }
+
         frame = makeUavcanFrame(sockcan_frame);
         /*
          * Timestamp
@@ -243,10 +256,6 @@ class SocketCanIface : public uavcan::ICanIface
             assert(0);
             return -1;
         }
-        /*
-         * Flags
-         */
-        loopback = (msg.msg_flags & static_cast<int>(MSG_CONFIRM)) != 0;
         return 1;
     }
 
@@ -315,6 +324,28 @@ class SocketCanIface : public uavcan::ICanIface
         }
     }
 
+    /**
+     * Returns true if a frame accepted by HW filters
+     */
+    bool checkHWFilters(const ::can_frame& frame) const
+    {
+        if (!hw_filters_container_.empty())
+        {
+            for (auto& f : hw_filters_container_)
+            {
+                if (((frame.can_id & f.can_mask) ^ f.can_id) == 0)
+                {
+                    return true;
+                }
+            }
+            return false;
+        }
+        else
+        {
+            return true;
+        }
+    }
+
 public:
     /**
      * Takes ownership of socket's file descriptor.
@@ -404,38 +435,42 @@ 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;
+            hw_filters_container_[i].can_id   = fc.id   & uavcan::CanFrame::MaskExtID;
-            filts[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
+            hw_filters_container_[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID;
             if (fc.id & uavcan::CanFrame::FlagEFF)
             {
-                filts[i].can_id |= CAN_EFF_FLAG;
+                hw_filters_container_[i].can_id |= CAN_EFF_FLAG;
             }
             if (fc.id & uavcan::CanFrame::FlagRTR)
             {
-                filts[i].can_id |= CAN_RTR_FLAG;
+                hw_filters_container_[i].can_id |= CAN_RTR_FLAG;
             }
             if (fc.mask & uavcan::CanFrame::FlagEFF)
             {
-                filts[i].can_mask |= CAN_EFF_FLAG;
+                hw_filters_container_[i].can_mask |= CAN_EFF_FLAG;
             }
             if (fc.mask & uavcan::CanFrame::FlagRTR)
             {
-                filts[i].can_mask |= CAN_RTR_FLAG;
+                hw_filters_container_[i].can_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;
+        return 0;
     }
 
     /**
      * 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; }
+    static constexpr unsigned NumFilters = 8;
+    std::uint16_t getNumFilters() const override { return NumFilters; }
+
 
     /**
      * Returns total number of errors of each kind detected since the object was created.
@@ -450,7 +485,7 @@ public:
     /**
      * Returns number of errors of each kind in a map.
      */
-    const decltype(errors_)& getErrors() const { return errors_; }
+    const decltype(errors_) & getErrors() const { return errors_; }
 
     int getFileDescriptor() const { return fd_; }
 
@@ -501,7 +536,7 @@ public:
                 goto fail;
             }
             // Non-blocking
-            if (::fcntl(s, F_SETFL , O_NONBLOCK) < 0)
+            if (::fcntl(s, F_SETFL, O_NONBLOCK) < 0)
             {
                 goto fail;
             }