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


ethercat_hardware
Author(s): Rob Wheeler (email: wheeler@willowgarage.com), Maintained by Derek King (email: dking@willowgarage.com)
autogenerated on Thu Apr 24 2014 15:43:44