uc_can_acceptance_filter_configurator.cpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>,
00003  *                    Ilia  Sheremet <illia.sheremet@gmail.com>
00004  */
00005 
00006 #include <uavcan/transport/can_acceptance_filter_configurator.hpp>
00007 #include <cassert>
00008 
00009 namespace uavcan
00010 {
00011 const unsigned CanAcceptanceFilterConfigurator::DefaultFilterMsgMask;
00012 const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceID;
00013 const unsigned CanAcceptanceFilterConfigurator::DefaultFilterServiceMask;
00014 const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgMask;
00015 const unsigned CanAcceptanceFilterConfigurator::DefaultAnonMsgID;
00016 
00017 int16_t CanAcceptanceFilterConfigurator::loadInputConfiguration(AnonymousMessages load_mode)
00018 {
00019     multiset_configs_.clear();
00020 
00021     if (load_mode == AcceptAnonymousMessages)
00022     {
00023         CanFilterConfig anon_frame_cfg;
00024         anon_frame_cfg.id = DefaultAnonMsgID | CanFrame::FlagEFF;
00025         anon_frame_cfg.mask = DefaultAnonMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR;
00026         if (multiset_configs_.emplace(anon_frame_cfg) == UAVCAN_NULLPTR)
00027         {
00028             return -ErrMemory;
00029         }
00030     }
00031 
00032     CanFilterConfig service_cfg;
00033     service_cfg.id = DefaultFilterServiceID;
00034     service_cfg.id |= (static_cast<uint32_t>(node_.getNodeID().get()) << 8) | CanFrame::FlagEFF;
00035     service_cfg.mask = DefaultFilterServiceMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR;
00036     if (multiset_configs_.emplace(service_cfg) == UAVCAN_NULLPTR)
00037     {
00038         return -ErrMemory;
00039     }
00040 
00041     const TransferListener* p = node_.getDispatcher().getListOfMessageListeners().get();
00042     while (p != UAVCAN_NULLPTR)
00043     {
00044         CanFilterConfig cfg;
00045         cfg.id = (static_cast<uint32_t>(p->getDataTypeDescriptor().getID().get()) << 8) | CanFrame::FlagEFF;
00046         cfg.mask = DefaultFilterMsgMask | CanFrame::FlagEFF | CanFrame::FlagRTR | CanFrame::FlagERR;
00047         if (multiset_configs_.emplace(cfg) == UAVCAN_NULLPTR)
00048         {
00049             return -ErrMemory;
00050         }
00051         p = p->getNextListNode();
00052     }
00053 
00054     if (multiset_configs_.getSize() == 0)
00055     {
00056         return -ErrLogic;
00057     }
00058 
00059     return 0;
00060 }
00061 
00062 int16_t CanAcceptanceFilterConfigurator::mergeConfigurations()
00063 {
00064     const uint16_t acceptance_filters_number = getNumFilters();
00065     if (acceptance_filters_number == 0)
00066     {
00067         UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
00068         return -ErrDriver;
00069     }
00070     UAVCAN_ASSERT(multiset_configs_.getSize() != 0);
00071 
00072     while (acceptance_filters_number < multiset_configs_.getSize())
00073     {
00074         uint16_t i_rank = 0, j_rank = 0;
00075         uint8_t best_rank = 0;
00076 
00077         const uint16_t multiset_array_size = static_cast<uint16_t>(multiset_configs_.getSize());
00078 
00079         for (uint16_t i_ind = 0; i_ind < multiset_array_size - 1; i_ind++)
00080         {
00081             for (uint16_t j_ind = static_cast<uint8_t>(i_ind + 1); j_ind < multiset_array_size; j_ind++)
00082             {
00083                 CanFilterConfig temp_config = mergeFilters(*multiset_configs_.getByIndex(i_ind),
00084                                                            *multiset_configs_.getByIndex(j_ind));
00085                 uint8_t rank = countBits(temp_config.mask);
00086                 if (rank > best_rank)
00087                 {
00088                     best_rank = rank;
00089                     i_rank = i_ind;
00090                     j_rank = j_ind;
00091                 }
00092             }
00093         }
00094 
00095         *multiset_configs_.getByIndex(j_rank) = mergeFilters(*multiset_configs_.getByIndex(i_rank),
00096                                                              *multiset_configs_.getByIndex(j_rank));
00097         multiset_configs_.removeFirst(*multiset_configs_.getByIndex(i_rank));
00098     }
00099 
00100     UAVCAN_ASSERT(acceptance_filters_number >= multiset_configs_.getSize());
00101 
00102     return 0;
00103 }
00104 
00105 int CanAcceptanceFilterConfigurator::applyConfiguration(void)
00106 {
00107     CanFilterConfig filter_conf_array[MaxCanAcceptanceFilters];
00108     unsigned int filter_array_size = multiset_configs_.getSize();
00109 
00110     const uint16_t acceptance_filters_number = getNumFilters();
00111     if (acceptance_filters_number == 0)
00112     {
00113         UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
00114         return -ErrDriver;
00115     }
00116 
00117     if (filter_array_size > acceptance_filters_number)
00118     {
00119         UAVCAN_TRACE("CanAcceptanceFilter", "Too many filter configurations. Executing computeConfiguration()");
00120         mergeConfigurations();
00121         filter_array_size = multiset_configs_.getSize();
00122     }
00123 
00124     if (filter_array_size > MaxCanAcceptanceFilters)
00125     {
00126         UAVCAN_ASSERT(0);
00127         UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
00128         return -ErrLogic;
00129     }
00130 
00131     for (uint16_t i = 0; i < filter_array_size; i++)
00132     {
00133         CanFilterConfig temp_filter_config = *multiset_configs_.getByIndex(i);
00134 
00135         filter_conf_array[i] = temp_filter_config;
00136     }
00137 
00138 #if UAVCAN_DEBUG
00139     for (uint16_t i = 0; i < multiset_configs_.getSize(); i++)
00140     {
00141         UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.ID [%u] = %u", i,
00142                      multiset_configs_.getByIndex(i)->id);
00143         UAVCAN_TRACE("CanAcceptanceFilterConfigurator::applyConfiguration()", "cfg.MK [%u] = %u", i,
00144                      multiset_configs_.getByIndex(i)->mask);
00145     }
00146 #endif
00147 
00148     ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver();
00149     for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
00150     {
00151         ICanIface* iface = can_driver.getIface(i);
00152         if (iface == UAVCAN_NULLPTR)
00153         {
00154             UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
00155             return -ErrDriver;
00156         }
00157         int16_t num = iface->configureFilters(reinterpret_cast<CanFilterConfig*>(&filter_conf_array),
00158                                               static_cast<uint16_t>(filter_array_size));
00159         if (num < 0)
00160         {
00161             UAVCAN_TRACE("CanAcceptanceFilter", "Failed to apply HW filter configuration");
00162             return -ErrDriver;
00163         }
00164     }
00165 
00166     return 0;
00167 }
00168 
00169 int CanAcceptanceFilterConfigurator::computeConfiguration(AnonymousMessages mode)
00170 {
00171     if (getNumFilters() == 0)
00172     {
00173         UAVCAN_TRACE("CanAcceptanceFilter", "No HW filters available");
00174         return -ErrDriver;
00175     }
00176 
00177     int16_t fill_array_error = loadInputConfiguration(mode);
00178     if (fill_array_error != 0)
00179     {
00180         UAVCAN_TRACE("CanAcceptanceFilter::loadInputConfiguration", "Failed to execute loadInputConfiguration()");
00181         return fill_array_error;
00182     }
00183 
00184     int16_t merge_configurations_error = mergeConfigurations();
00185     if (merge_configurations_error != 0)
00186     {
00187         UAVCAN_TRACE("CanAcceptanceFilter", "Failed to compute optimal acceptance fliter's configuration");
00188         return merge_configurations_error;
00189     }
00190 
00191     return 0;
00192 }
00193 
00194 uint16_t CanAcceptanceFilterConfigurator::getNumFilters() const
00195 {
00196     if (filters_number_ == 0)
00197     {
00198         static const uint16_t InvalidOut = 0xFFFF;
00199         uint16_t out = InvalidOut;
00200         ICanDriver& can_driver = node_.getDispatcher().getCanIOManager().getCanDriver();
00201 
00202         for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++)
00203         {
00204             const ICanIface* iface = can_driver.getIface(i);
00205             if (iface == UAVCAN_NULLPTR)
00206             {
00207                 UAVCAN_ASSERT(0);
00208                 out = 0;
00209                 break;
00210             }
00211             const uint16_t num = iface->getNumFilters();
00212             out = min(out, num);
00213             if (out > MaxCanAcceptanceFilters)
00214             {
00215                 out = MaxCanAcceptanceFilters;
00216             }
00217         }
00218 
00219         return (out == InvalidOut) ? 0 : out;
00220     }
00221     else
00222     {
00223         return filters_number_;
00224     }
00225 }
00226 
00227 int CanAcceptanceFilterConfigurator::addFilterConfig(const CanFilterConfig& config)
00228 {
00229     if (multiset_configs_.emplace<const CanFilterConfig&>(config) == UAVCAN_NULLPTR)
00230     {
00231         return -ErrMemory;
00232     }
00233 
00234     return 0;
00235 }
00236 
00237 CanFilterConfig CanAcceptanceFilterConfigurator::mergeFilters(CanFilterConfig& a_, CanFilterConfig& b_)
00238 {
00239     CanFilterConfig temp_arr;
00240     temp_arr.mask = a_.mask & b_.mask & ~(a_.id ^ b_.id);
00241     temp_arr.id = a_.id & temp_arr.mask;
00242 
00243     return temp_arr;
00244 }
00245 
00246 uint8_t CanAcceptanceFilterConfigurator::countBits(uint32_t n_)
00247 {
00248     uint8_t c_; // c accumulates the total bits set in v
00249     for (c_ = 0; n_; c_++)
00250     {
00251         n_ &= n_ - 1; // clear the least significant bit set
00252     }
00253 
00254     return c_;
00255 }
00256 }