ethernet_interface_info.cpp
Go to the documentation of this file.
00001 #include "ros_ethercat_hardware/ethernet_interface_info.h"
00002 #include <linux/ethtool.h>
00003 #include <linux/sockios.h>
00004 #include <net/if.h>
00005 #include <sys/ioctl.h>
00006 #include <errno.h>
00007 
00008 EthtoolStats::EthtoolStats() :
00009   rx_errors_(0),
00010   rx_crc_errors_(0),
00011   rx_frame_errors_(0),
00012   rx_align_errors_(0)
00013 {
00014   //empty
00015 }
00016 
00017 EthtoolStats& EthtoolStats::operator-=(const EthtoolStats& right)
00018 {
00019   this->rx_errors_ -= right.rx_errors_;
00020   this->rx_crc_errors_ -= right.rx_crc_errors_;
00021   this->rx_frame_errors_ -= right.rx_frame_errors_;
00022   this->rx_align_errors_ -= right.rx_align_errors_;
00023   return *this;
00024 }
00025 
00026 EthernetInterfaceInfo::EthernetInterfaceInfo() :
00027   sock_(-1),
00028   n_stats_(0),
00029   ethtool_stats_buf_(NULL),
00030   rx_error_index_(-1),
00031   rx_crc_error_index_(-1),
00032   rx_frame_error_index_(-1),
00033   rx_align_error_index_(-1)
00034 {
00035 }
00036 
00037 EthernetInterfaceInfo::~EthernetInterfaceInfo()
00038 {
00039   delete[] ethtool_stats_buf_;
00040   ethtool_stats_buf_ = NULL;
00041   if (sock_ >= 0)
00042     close(sock_);
00043 }
00044 
00045 void EthernetInterfaceInfo::initialize(const std::string &interface)
00046 {
00047   interface_ = interface;
00048 
00049   // Need network socket to make interface requests ioctls
00050   sock_ = socket(AF_INET, SOCK_DGRAM, 0);
00051   if (sock_ < 0)
00052   {
00053     ROS_WARN("Cannot get control socket for ioctls : %s", strerror(errno));
00054     return;
00055   }
00056 
00057   // Get initial interface state
00058   getInterfaceState(last_state_);
00059 
00060   struct ifreq ifr;
00061   memset(&ifr, 0, sizeof (ifr));
00062   strncpy(ifr.ifr_name, interface_.c_str(), sizeof (ifr.ifr_name));
00063 
00064   // Determine number of statictics available from network interface
00065   struct ethtool_drvinfo drvinfo;
00066   drvinfo.cmd = ETHTOOL_GDRVINFO;
00067   ifr.ifr_data = (caddr_t) & drvinfo;
00068   int result = ioctl(sock_, SIOCETHTOOL, &ifr);
00069   if (result < 0)
00070   {
00071     ROS_WARN("Cannot get driver information for %s : %s", interface_.c_str(), strerror(errno));
00072     return;
00073   }
00074   n_stats_ = drvinfo.n_stats;
00075   if (n_stats_ < 1)
00076   {
00077     ROS_WARN("No NIC statistics available for %s", interface_.c_str());
00078     return;
00079   }
00080 
00081   unsigned strings_len = sizeof (ethtool_gstrings) + n_stats_ * ETH_GSTRING_LEN;
00082   char *strings_buf = new char[strings_len];
00083   memset(strings_buf, 0, strings_len);
00084   ethtool_gstrings* strings = (ethtool_gstrings*) strings_buf;
00085 
00086   strings->cmd = ETHTOOL_GSTRINGS;
00087   strings->string_set = ETH_SS_STATS;
00088   strings->len = n_stats_;
00089   ifr.ifr_data = (caddr_t) strings;
00090   result = ioctl(sock_, SIOCETHTOOL, &ifr);
00091   if (result < 0)
00092   {
00093     ROS_WARN("Cannot get statistics strings for %s : %s", interface_.c_str(), strerror(errno));
00094     delete[] strings_buf;
00095     return;
00096   }
00097 
00098   for (unsigned i = 0; i < n_stats_; ++i)
00099   {
00100     if (false)
00101     {
00102       char s[ETH_GSTRING_LEN + 1];
00103       strncpy(s, (const char*) &strings->data[i * ETH_GSTRING_LEN], ETH_GSTRING_LEN);
00104       s[ETH_GSTRING_LEN] = '\0';
00105       ROS_WARN("Stat %i : %s", i, s);
00106     }
00107     const char *stat_name = (const char*) &strings->data[i * ETH_GSTRING_LEN];
00108     if (strncmp("rx_errors", stat_name, ETH_GSTRING_LEN) == 0)
00109     {
00110       rx_error_index_ = i;
00111     }
00112     else if (strncmp("rx_crc_errors", stat_name, ETH_GSTRING_LEN) == 0)
00113     {
00114       rx_crc_error_index_ = i;
00115     }
00116     else if (strncmp("rx_frame_errors", stat_name, ETH_GSTRING_LEN) == 0)
00117     {
00118       rx_frame_error_index_ = i;
00119     }
00120     else if (strncmp("rx_align_errors", stat_name, ETH_GSTRING_LEN) == 0)
00121     {
00122       rx_align_error_index_ = i;
00123     }
00124   }
00125 
00126   // Everything is complete, allocate memory for ethtool_stats_ buffer
00127   // Since not all NICs provide ethtool statistics, use the presence of
00128   // ethtool_stats_ buffer to indicate initialization was a success.
00129   unsigned ethtool_stats_buf_len = sizeof (struct ethtool_stats) +n_stats_ * sizeof (uint64_t);
00130   ethtool_stats_buf_ = new char[ethtool_stats_buf_len];
00131 
00132   if (!getEthtoolStats(orig_stats_))
00133   {
00134     // Don't run if we can't get initial statitics
00135     ROS_WARN("Error collecting intial ethernet interface statistics");
00136     delete[] ethtool_stats_buf_;
00137     ethtool_stats_buf_ = NULL;
00138   }
00139 }
00140 
00141 bool EthernetInterfaceInfo::getInterfaceState(InterfaceState &state)
00142 {
00143   struct ifreq ifr;
00144   memset(&ifr, 0, sizeof (ifr));
00145   strncpy(ifr.ifr_name, interface_.c_str(), sizeof (ifr.ifr_name));
00146 
00147   if (ioctl(sock_, SIOCGIFFLAGS, &ifr) < 0)
00148   {
00149     ROS_WARN("Cannot get interface flags for %s: %s", interface_.c_str(), strerror(errno));
00150     return false;
00151   }
00152 
00153   state.up_ = bool(ifr.ifr_flags & IFF_UP);
00154   state.running_ = bool(ifr.ifr_flags & IFF_RUNNING);
00155   return true;
00156 }
00157 
00158 bool EthernetInterfaceInfo::getEthtoolStats(EthtoolStats &s)
00159 {
00160   if (!ethtool_stats_buf_)
00161     return false;
00162 
00163   struct ifreq ifr;
00164   memset(&ifr, 0, sizeof (ifr));
00165   strncpy(ifr.ifr_name, interface_.c_str(), sizeof (ifr.ifr_name));
00166 
00167   struct ethtool_stats *stats = (struct ethtool_stats *) ethtool_stats_buf_;
00168   stats->cmd = ETHTOOL_GSTATS;
00169   stats->n_stats = n_stats_;
00170   ifr.ifr_data = (caddr_t) stats;
00171   if (ioctl(sock_, SIOCETHTOOL, &ifr) < 0)
00172   {
00173     ROS_WARN("Cannot get NIC stats information for %s : %s", interface_.c_str(), strerror(errno));
00174     return false;
00175   }
00176 
00177   if (rx_error_index_ >= 0)
00178   {
00179     s.rx_errors_ = stats->data[rx_error_index_];
00180   }
00181   if (rx_crc_error_index_ >= 0)
00182   {
00183     s.rx_crc_errors_ = stats->data[rx_crc_error_index_];
00184   }
00185   if (rx_frame_error_index_ >= 0)
00186   {
00187     s.rx_frame_errors_ = stats->data[rx_frame_error_index_];
00188   }
00189   if (rx_align_error_index_ >= 0)
00190   {
00191     s.rx_align_errors_ = stats->data[rx_align_error_index_];
00192   }
00193 
00194   return true;
00195 }
00196 
00197 void EthernetInterfaceInfo::publishDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d)
00198 {
00199   d.add("Interface", interface_);
00200 
00201   // TODO : collect and publish information on whether interface is up/running
00202   InterfaceState state;
00203   if (getInterfaceState(state))
00204   {
00205     if (!state.running_ && last_state_.running_)
00206     {
00207       ++lost_link_count_;
00208     }
00209 
00210     if (state.up_ && !state.running_)
00211     {
00212       d.mergeSummary(d.ERROR, "No link");
00213     }
00214     else if (!state.up_)
00215     {
00216       d.mergeSummary(d.ERROR, "Interface down");
00217     }
00218 
00219     d.addf("Interface State", "%s UP, %s RUNNING", state.up_ ? "" : "NOT",
00220            state.running_ ? "" : "NOT");
00221     last_state_ = state;
00222   }
00223   else
00224   {
00225     d.add("Iface State", "ERROR");
00226   }
00227   d.add("Lost Links", lost_link_count_);
00228 
00229   EthtoolStats stats;
00230   bool have_stats = getEthtoolStats(stats);
00231   stats -= orig_stats_; //subtract off orignal counter values
00232 
00233   if (have_stats && (rx_error_index_ >= 0))
00234     d.addf("RX Errors", "%llu", stats.rx_errors_);
00235   else
00236     d.add("RX Errors", "N/A");
00237 
00238   if (have_stats && (rx_crc_error_index_ >= 0))
00239     d.addf("RX CRC Errors", "%llu", stats.rx_crc_errors_);
00240   else
00241     d.add("RX CRC Errors", "N/A");
00242 
00243   if (have_stats && (rx_frame_error_index_ >= 0))
00244     d.addf("RX Frame Errors", "%llu", stats.rx_frame_errors_);
00245   else
00246     d.add("RX Frame Errors", "N/A");
00247 
00248   if (have_stats && (rx_align_error_index_ >= 0))
00249     d.addf("RX Align Errors", "%llu", stats.rx_align_errors_);
00250   else
00251     d.add("RX Align Errors", "N/A");
00252 
00253 }


ros_ethercat_hardware
Author(s): Rob Wheeler , Derek King , Manos Nikolaidis
autogenerated on Thu Jul 4 2019 20:01:53