git.gag.com Git - debian/gnuradio/blob - usrp2/host/lib/ethernet.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2005,2007,2008 Free Software Foundation, Inc.
   4  *
   5  * This program is free software: you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation, either version 3 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
  17  */
  18
  19 #ifdef HAVE_CONFIG_H
  20 #include <config.h>
  21 #endif
  22
  23 #include "eth_buffer.h"
  24 #include "ethernet.h"
  25 #include "pktfilter.h"
  26 #include <open_usrp2_socket.h>
  27
  28 #include <cstdio>
  29 #include <iostream>
  30 #include <unistd.h>
  31 #include <fcntl.h>
  32 //#include <features.h>
  33 #include <errno.h>
  34 #include <string.h>
  35 #include <net/if.h>
  36 #include <sys/ioctl.h>
  37 #include <sys/socket.h>
  38 #include <net/ethernet.h>
  39 #include <netinet/in.h>
  40
  41 #include <linux/types.h>
  42 #include <netpacket/packet.h>
  43 #include <linux/filter.h>       // packet filter
  44
  45 namespace usrp2 {
  46
  47   static int
  48   open_packet_socket (std::string ifname, int protocol)
  49   {
  50 #if 0
  51     if (protocol == 0)
  52       protocol = htons(ETH_P_ALL);
  53
  54     int fd = socket (PF_PACKET, SOCK_RAW, protocol);
  55 #else
  56     int fd = usrp2::open_usrp2_socket();
  57 #endif
  58
  59     if (fd == -1){
  60       fprintf (stderr, "%s: socket: %s\n", ifname.c_str(), strerror (errno));
  61       return -1;
  62     }
  63
  64     // get interface index
  65     struct ifreq ifr;
  66     memset (&ifr, 0, sizeof(ifr));
  67     strncpy (ifr.ifr_name, ifname.c_str(), sizeof (ifr.ifr_name));
  68     int res = ioctl (fd, SIOCGIFINDEX, &ifr);
  69     if (res != 0){
  70       ::close (fd);
  71       fprintf (stderr, "%s: SIOCGIFINDEX: %s\n", ifname.c_str(), strerror(errno));
  72       return -1;
  73     }
  74     int ifindex = ifr.ifr_ifindex;
  75
  76     // bind to the specified interface
  77     sockaddr_ll sa;
  78     memset (&sa, 0, sizeof (sa));
  79     sa.sll_family = AF_PACKET;
  80     sa.sll_protocol = protocol;
  81     sa.sll_ifindex = ifindex;
  82     res = bind (fd, (struct sockaddr *)&sa, sizeof (sa));
  83     if (res != 0){
  84       ::close (fd);
  85       fprintf (stderr, "%s: bind: %s\n", ifname.c_str(), strerror(errno));
  86       return -1;
  87     }
  88     return fd;
  89   }
  90
  91   static void
  92   extract_mac_addr (unsigned char *mac, const unsigned char *hwaddr)
  93   {
  94     int i;
  95     for (i = 0; i < 6; i++)
  96       mac[i] = 0xff;
  97
  98     i = 0;
  99     for (int j = 0; j < 14; j++){
 100       if (hwaddr[j] != 0xff){
 101         mac[i++] = hwaddr[j];
 102         if (i == 6)
 103           return;
 104       }
 105     }
 106   }
 107
 108   static bool
 109   get_mac_addr (std::string ifname, int fd, unsigned char *mac)
 110   {
 111     struct ifreq ifr;
 112     memset (&ifr, 0, sizeof(ifr));
 113     strncpy (ifr.ifr_name, ifname.c_str(), sizeof (ifr.ifr_name));
 114     int res = ioctl (fd, SIOCGIFHWADDR, &ifr);
 115     if (res != 0){
 116       fprintf (stderr, "%s: SIOCGIFHWADDR: %s\n", ifname.c_str(), strerror(errno));
 117       return false;
 118     }
 119     else {
 120       if (0){
 121         for (unsigned i = 0; i < sizeof (ifr.ifr_hwaddr.sa_data); i++)
 122           fprintf (stderr, "%02x", ifr.ifr_hwaddr.sa_data[i]);
 123         fprintf (stderr, "\n");
 124       }
 125     }
 126     extract_mac_addr (mac, (unsigned char *)ifr.ifr_hwaddr.sa_data);
 127     return true;
 128   }
 129
 130   ethernet::ethernet ()
 131   {
 132     d_fd = -1;
 133     memset (d_mac, 0, sizeof (d_mac));
 134   }
 135
 136   ethernet::~ethernet ()
 137   {
 138     close ();
 139   }
 140
 141   bool
 142   ethernet::open (std::string ifname, int protocol)
 143   {
 144     if (d_fd != -1){
 145       fprintf (stderr, "ethernet: already open\n");
 146       return false;
 147     }
 148     if ((d_fd = open_packet_socket (ifname, protocol)) == -1){
 149       return false;
 150     }
 151     get_mac_addr (ifname, d_fd, d_mac);
 152     return true;
 153   }
 154
 155   bool
 156   ethernet::close ()
 157   {
 158     if (d_fd >= 0){
 159       ::close (d_fd);
 160       d_fd = -1;
 161     }
 162     return true;
 163   }
 164
 165   int
 166   ethernet::read_packet (void *buf, int buflen)
 167   {
 168     int len = recvfrom (d_fd, buf, buflen, 0, (sockaddr *) 0, 0);
 169     return len;
 170   }
 171
 172   int
 173   ethernet::read_packet_dont_block (void *buf, int buflen)
 174   {
 175     int len = recvfrom (d_fd, buf, buflen, MSG_DONTWAIT, 0, 0);
 176     if (len == -1 && errno == EAGAIN)
 177       return 0;
 178
 179     return len;
 180   }
 181
 182   int
 183   ethernet::write_packet (const void *buf, int buflen)
 184   {
 185     int retval = send (d_fd, buf, buflen, 0);
 186     if (retval < 0){
 187       if (errno == EINTR)
 188         return write_packet (buf, buflen);
 189
 190       perror ("ethernet:write_packet: send");
 191       return -1;
 192     }
 193     return retval;
 194   }
 195
 196   int
 197   ethernet::write_packetv(const eth_iovec *iov, size_t iovlen)
 198   {
 199     struct msghdr mh;
 200     memset(&mh, 0, sizeof(mh));
 201     mh.msg_iov = const_cast<eth_iovec*>(iov);
 202     mh.msg_iovlen = iovlen;
 203
 204     int retval = sendmsg(d_fd, &mh, 0);
 205     if (retval < 0){
 206       if (errno == EINTR)
 207         return write_packetv(iov, iovlen);
 208
 209       perror("ethernet:write_packetv: send");
 210       return -1;
 211     }
 212     return retval;
 213   }
 214
 215   bool
 216   ethernet::attach_pktfilter (pktfilter *pf)
 217   {
 218     struct sock_fprog filter;
 219     filter.len = pf->d_len;
 220     filter.filter = pf->d_inst;
 221
 222     int r = setsockopt (d_fd, SOL_SOCKET, SO_ATTACH_FILTER, &filter, sizeof (filter));
 223     if (r < 0){
 224       perror ("ethernet:attach:  SO_ATTACH_FILTER");
 225       return false;
 226     }
 227     return true;
 228   }
 229
 230 } // namespace usrp2