3 * Copyright 2005,2007,2008 Free Software Foundation, Inc.
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.
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.
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/>.
24 #include "pktfilter.h"
25 #include <open_usrp2_socket.h>
30 //#include <features.h>
34 #include <sys/ioctl.h>
35 #include <sys/socket.h>
36 #include <net/ethernet.h>
37 #include <netinet/in.h>
39 #include <linux/types.h>
40 #include <netpacket/packet.h>
41 #include <linux/filter.h> // packet filter
46 open_packet_socket (std::string ifname, int protocol)
50 protocol = htons(ETH_P_ALL);
52 int fd = socket (PF_PACKET, SOCK_RAW, protocol);
54 int fd = usrp2::open_usrp2_socket();
58 fprintf (stderr, "%s: socket: %s\n", ifname.c_str(), strerror (errno));
62 // get interface index
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);
69 fprintf (stderr, "%s: SIOCGIFINDEX: %s\n", ifname.c_str(), strerror(errno));
72 int ifindex = ifr.ifr_ifindex;
74 // bind to the specified interface
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));
83 fprintf (stderr, "%s: bind: %s\n", ifname.c_str(), strerror(errno));
90 extract_mac_addr (unsigned char *mac, const unsigned char *hwaddr)
93 for (i = 0; i < 6; i++)
97 for (int j = 0; j < 14; j++){
98 if (hwaddr[j] != 0xff){
107 get_mac_addr (std::string ifname, int fd, unsigned char *mac)
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);
114 fprintf (stderr, "%s: SIOCGIFHWADDR: %s\n", ifname.c_str(), strerror(errno));
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");
124 extract_mac_addr (mac, (unsigned char *)ifr.ifr_hwaddr.sa_data);
128 ethernet::ethernet ()
131 memset (d_mac, 0, sizeof (d_mac));
134 ethernet::~ethernet ()
140 ethernet::open (std::string ifname, int protocol)
143 fprintf (stderr, "ethernet: already open\n");
146 if ((d_fd = open_packet_socket (ifname, protocol)) == -1){
149 get_mac_addr (ifname, d_fd, d_mac);
164 ethernet::read_packet (void *buf, int buflen)
166 int len = recvfrom (d_fd, buf, buflen, 0, (sockaddr *) 0, 0);
171 ethernet::read_packet_dont_block (void *buf, int buflen)
173 int len = recvfrom (d_fd, buf, buflen, MSG_DONTWAIT, 0, 0);
174 if (len == -1 && errno == EAGAIN)
181 ethernet::write_packet (const void *buf, int buflen)
183 int retval = send (d_fd, buf, buflen, 0);
186 return write_packet (buf, buflen);
188 perror ("ethernet:write_packet: send");
195 ethernet::write_packetv(const eth_iovec *iov, size_t iovlen)
198 memset(&mh, 0, sizeof(mh));
199 mh.msg_iov = const_cast<eth_iovec*>(iov);
200 mh.msg_iovlen = iovlen;
202 int retval = sendmsg(d_fd, &mh, 0);
205 return write_packetv(iov, iovlen);
207 perror("ethernet:write_packetv: send");
214 ethernet::attach_pktfilter (pktfilter *pf)
216 struct sock_fprog filter;
217 filter.len = pf->d_len;
218 filter.filter = pf->d_inst;
220 int r = setsockopt (d_fd, SOL_SOCKET, SO_ATTACH_FILTER, &filter, sizeof (filter));
222 perror ("ethernet:attach: SO_ATTACH_FILTER");