git.gag.com Git - debian/gnuradio/blob - usrp2/host/lib/pktfilter.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 "pktfilter.h"
  24 #include <iostream>
  25 #include <sys/ioctl.h>
  26 #include <sys/socket.h>
  27 #include <net/if.h>
  28 #include <features.h>
  29 #include <netpacket/packet.h>
  30 #include <net/ethernet.h>
  31 #include <netinet/in.h>
  32 #include <assert.h>
  33 #include <linux/types.h>
  34 #include <linux/filter.h>       // packet filter
  35
  36 namespace usrp2 {
  37
  38   /*
  39    * This is all based on the Berkeley Packet Filter (BPF) as implemented on Linux.
  40    *
  41    * The BPF allows you to run an interpreted program (a filter) in the
  42    * kernel that sorts through the packets looking for ones you are
  43    * interested in.  This eliminates the overhead of passing all of the
  44    * networking packets up into user space for filtering there.
  45    *
  46    * For documentation on this see
  47    * /usr/src/linux/Documentation/networking/filter.txt, The BSD
  48    * Berkeley Packet Filter manual page, and "The BSD Packet Filter: A
  49    * New Architecture for User-level Packet Capture", by Steven McCanne
  50    * and Van Jacobson.
  51    */
  52
  53   pktfilter::pktfilter ()
  54     : d_len (0), d_inst (0)
  55   {
  56     // NOP
  57   }
  58
  59   pktfilter::~pktfilter ()
  60   {
  61     delete [] d_inst;
  62   }
  63
  64   inline static sock_filter
  65   make_stmt (__u16 code, __u32 k)
  66   {
  67     sock_filter f;
  68     f.code = code;
  69     f.jt = 0;
  70     f.jf = 0;
  71     f.k = k;
  72     return f;
  73   }
  74
  75   inline static sock_filter
  76   make_jump (__u16 code, __u32 k, __u8 jt, __u8 jf)
  77   {
  78     sock_filter f;
  79     f.code = code;
  80     f.jt = jt;
  81     f.jf = jf;
  82     f.k = k;
  83     return f;
  84   }
  85
  86   /*
  87    * Return a filter that harvests packets with the specified ethertype.
  88    */
  89   pktfilter *
  90   pktfilter::make_ethertype (unsigned short ethertype)
  91   {
  92     static const int MAX_LEN = 20;
  93     sock_filter *inst = new sock_filter [MAX_LEN];
  94     pktfilter   *pf = new pktfilter ();
  95
  96     // nothing quite like coding in assembly without the benefit of an assembler ;-)
  97
  98     // ignore packets that don't have the right ethertype
  99
 100     int i = 0;
 101     inst[i++] = make_stmt (BPF_LD|BPF_H|BPF_ABS, 12);   // load ethertype
 102     inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, ethertype, 1, 0);
 103     inst[i++] = make_stmt (BPF_RET|BPF_K, 0);           // return 0 (ignore packet)
 104     inst[i++] = make_stmt (BPF_RET|BPF_K, (unsigned) -1);       // return whole packet
 105
 106     assert (i <= MAX_LEN);
 107
 108     pf->d_inst = inst;
 109     pf->d_len = i;
 110
 111     return pf;
 112   }
 113
 114   /*
 115    * Return a filter that harvests inbound packets with the specified ethertype.
 116    * \param ethertype   the ethertype we're looking for
 117    * \param our_mac     our ethernet MAC address so we can avoid pkts we sent
 118    */
 119   pktfilter *
 120   pktfilter::make_ethertype_inbound (unsigned short ethertype, const unsigned char *our_mac)
 121   {
 122     static const int MAX_LEN = 20;
 123     sock_filter *inst = new sock_filter [MAX_LEN];
 124     pktfilter   *pf = new pktfilter ();
 125
 126     __u16 smac_hi = (our_mac[0] << 8) | our_mac[1];
 127     __u32 smac_lo = (our_mac[2] << 24) | (our_mac[3] << 16) | (our_mac[4] << 8) | our_mac[5];
 128
 129     // nothing quite like coding in assembly without the benefit of an assembler ;-)
 130
 131     // ignore packets that have a different ethertype
 132     // and packets that have a source mac address == our_mac (packets we sent)
 133
 134     int i = 0;
 135     inst[i++] = make_stmt (BPF_LD|BPF_H|BPF_ABS, 12);   // load ethertype
 136     inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, ethertype, 0, 5);
 137     inst[i++] = make_stmt (BPF_LD|BPF_W|BPF_ABS, 8);    // load low 32-bit of src mac
 138     inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, smac_lo, 0, 2);
 139     inst[i++] = make_stmt (BPF_LD|BPF_H|BPF_ABS, 6);    // load high 16-bits of src mac
 140     inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, smac_hi, 1, 0);
 141     inst[i++] = make_stmt (BPF_RET|BPF_K, (unsigned) -1);       // return whole packet
 142     inst[i++] = make_stmt (BPF_RET|BPF_K, 0);           // return 0 (ignore packet)
 143
 144     assert (i <= MAX_LEN);
 145
 146     pf->d_inst = inst;
 147     pf->d_len = i;
 148
 149     return pf;
 150   }
 151   /*
 152    * Return a filter that harvests inbound packets with the specified ethertype and target USRP2 MAC address.
 153    * \param ethertype   the ethertype we're looking for
 154    * \param usrp_mac    our target USRP2 MAC address
 155    */
 156   pktfilter *
 157   pktfilter::make_ethertype_inbound_target (unsigned short ethertype, const unsigned char *usrp_mac)
 158   {
 159     static const int MAX_LEN = 20;
 160     sock_filter *inst = new sock_filter [MAX_LEN];
 161     pktfilter   *pf = new pktfilter ();
 162
 163     __u16 tmac_hi = (usrp_mac[0] << 8) | usrp_mac[1];
 164     __u32 tmac_lo = (usrp_mac[2] << 24) | (usrp_mac[3] << 16) | (usrp_mac[4] << 8) | usrp_mac[5];
 165
 166     // ignore packets that have a different ethertype
 167     // and only return packets that have a source mac address == usrp_mac
 168
 169     int i = 0;
 170     inst[i++] = make_stmt (BPF_LD|BPF_H|BPF_ABS, 12);   // load ethertype
 171     inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, ethertype, 0, 5);
 172     inst[i++] = make_stmt (BPF_LD|BPF_W|BPF_ABS, 8);    // load low 32-bit of src mac
 173     inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, tmac_lo, 0, 3);
 174     inst[i++] = make_stmt (BPF_LD|BPF_H|BPF_ABS, 6);    // load high 16-bits of src mac
 175     inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, tmac_hi, 0, 1);
 176     inst[i++] = make_stmt (BPF_RET|BPF_K, (unsigned) -1);       // return whole packet
 177     inst[i++] = make_stmt (BPF_RET|BPF_K, 0);           // return 0 (ignore packet)
 178
 179     assert (i <= MAX_LEN);
 180
 181     pf->d_inst = inst;
 182     pf->d_len = i;
 183
 184     return pf;
 185   }
 186
 187 } // namespace usrp2