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 "ethernet.h"
  24 #include "pktfilter.h"
  25 #include <open_usrp2_socket.h>
  26
  27 #include <iostream>
  28 #include <unistd.h>
  29 #include <fcntl.h>
  30 //#include <features.h>
  31 #include <errno.h>
  32 #include <string.h>
  33 #include <net/if.h>
  34 #include <sys/ioctl.h>
  35 #include <sys/socket.h>
  36 #include <net/ethernet.h>
  37 #include <netinet/in.h>
  38
  39 #include <linux/types.h>
  40 #include <netpacket/packet.h>
  41 #include <linux/filter.h>       // packet filter
  42
  43 namespace usrp2 {
  44
  45   static int
  46   open_packet_socket (std::string ifname, int protocol)
  47   {
  48 #if 0
  49     if (protocol == 0)
  50       protocol = htons(ETH_P_ALL);
  51
  52     int fd = socket (PF_PACKET, SOCK_RAW, protocol);
  53 #else
  54     int fd = usrp2::open_usrp2_socket();
  55 #endif
  56
  57     if (fd == -1){
  58       fprintf (stderr, "%s: socket: %s\n", ifname.c_str(), strerror (errno));
  59       return -1;
  60     }
  61
  62     // get interface index
  63     struct ifreq ifr;
  64     memset (&ifr, 0, sizeof(ifr));
  65     strncpy (ifr.ifr_name, ifname.c_str(), sizeof (ifr.ifr_name));
  66     int res = ioctl (fd, SIOCGIFINDEX, &ifr);
  67     if (res != 0){
  68       ::close (fd);
  69       fprintf (stderr, "%s: SIOCGIFINDEX: %s\n", ifname.c_str(), strerror(errno));
  70       return -1;
  71     }
  72     int ifindex = ifr.ifr_ifindex;
  73
  74     // bind to the specified interface
  75     sockaddr_ll sa;
  76     memset (&sa, 0, sizeof (sa));
  77     sa.sll_family = AF_PACKET;
  78     sa.sll_protocol = protocol;
  79     sa.sll_ifindex = ifindex;
  80     res = bind (fd, (struct sockaddr *)&sa, sizeof (sa));
  81     if (res != 0){
  82       ::close (fd);
  83       fprintf (stderr, "%s: bind: %s\n", ifname.c_str(), strerror(errno));
  84       return -1;
  85     }
  86     return fd;
  87   }
  88
  89   static void
  90   extract_mac_addr (unsigned char *mac, const unsigned char *hwaddr)
  91   {
  92     int i;
  93     for (i = 0; i < 6; i++)
  94       mac[i] = 0xff;
  95
  96     i = 0;
  97     for (int j = 0; j < 14; j++){
  98       if (hwaddr[j] != 0xff){
  99         mac[i++] = hwaddr[j];
 100         if (i == 6)
 101           return;
 102       }
 103     }
 104   }
 105
 106   static bool
 107   get_mac_addr (std::string ifname, int fd, unsigned char *mac)
 108   {
 109     struct ifreq ifr;
 110     memset (&ifr, 0, sizeof(ifr));
 111     strncpy (ifr.ifr_name, ifname.c_str(), sizeof (ifr.ifr_name));
 112     int res = ioctl (fd, SIOCGIFHWADDR, &ifr);
 113     if (res != 0){
 114       fprintf (stderr, "%s: SIOCGIFHWADDR: %s\n", ifname.c_str(), strerror(errno));
 115       return false;
 116     }
 117     else {
 118       if (0){
 119         for (unsigned i = 0; i < sizeof (ifr.ifr_hwaddr.sa_data); i++)
 120           fprintf (stderr, "%02x", ifr.ifr_hwaddr.sa_data[i]);
 121         fprintf (stderr, "\n");
 122       }
 123     }
 124     extract_mac_addr (mac, (unsigned char *)ifr.ifr_hwaddr.sa_data);
 125     return true;
 126   }
 127
 128   ethernet::ethernet ()
 129   {
 130     d_fd = -1;
 131     memset (d_mac, 0, sizeof (d_mac));
 132   }
 133
 134   ethernet::~ethernet ()
 135   {
 136     close ();
 137   }
 138
 139   bool
 140   ethernet::open (std::string ifname, int protocol)
 141   {
 142     if (d_fd != -1){
 143       fprintf (stderr, "ethernet: already open\n");
 144       return false;
 145     }
 146     if ((d_fd = open_packet_socket (ifname, protocol)) == -1){
 147       return false;
 148     }
 149     get_mac_addr (ifname, d_fd, d_mac);
 150     return true;
 151   }
 152
 153   bool
 154   ethernet::close ()
 155   {
 156     if (d_fd >= 0){
 157       ::close (d_fd);
 158       d_fd = -1;
 159     }
 160     return true;
 161   }
 162
 163   int
 164   ethernet::read_packet (void *buf, int buflen)
 165   {
 166     int len = recvfrom (d_fd, buf, buflen, 0, (sockaddr *) 0, 0);
 167     return len;
 168   }
 169
 170   int
 171   ethernet::read_packet_dont_block (void *buf, int buflen)
 172   {
 173     int len = recvfrom (d_fd, buf, buflen, MSG_DONTWAIT, 0, 0);
 174     if (len == -1 && errno == EAGAIN)
 175       return 0;
 176
 177     return len;
 178   }
 179
 180   int
 181   ethernet::write_packet (const void *buf, int buflen)
 182   {
 183     int retval = send (d_fd, buf, buflen, 0);
 184     if (retval < 0){
 185       if (errno == EINTR)
 186         return write_packet (buf, buflen);
 187
 188       perror ("ethernet:write_packet: send");
 189       return -1;
 190     }
 191     return retval;
 192   }
 193
 194   int
 195   ethernet::write_packetv(const eth_iovec *iov, size_t iovlen)
 196   {
 197     struct msghdr mh;
 198     memset(&mh, 0, sizeof(mh));
 199     mh.msg_iov = const_cast<eth_iovec*>(iov);
 200     mh.msg_iovlen = iovlen;
 201
 202     int retval = sendmsg(d_fd, &mh, 0);
 203     if (retval < 0){
 204       if (errno == EINTR)
 205         return write_packetv(iov, iovlen);
 206
 207       perror("ethernet:write_packetv: send");
 208       return -1;
 209     }
 210     return retval;
 211   }
 212
 213   bool
 214   ethernet::attach_pktfilter (pktfilter *pf)
 215   {
 216     struct sock_fprog filter;
 217     filter.len = pf->d_len;
 218     filter.filter = pf->d_inst;
 219
 220     int r = setsockopt (d_fd, SOL_SOCKET, SO_ATTACH_FILTER, &filter, sizeof (filter));
 221     if (r < 0){
 222       perror ("ethernet:attach:  SO_ATTACH_FILTER");
 223       return false;
 224     }
 225     return true;
 226   }
 227
 228 } // namespace usrp2