[Commit-gnuradio] r10645 - gnuradio/trunk/usrp2/host/lib

[jcorgan]

Author: jcorgan
Date: 2009-03-19 11:09:20 -0600 (Thu, 19 Mar 2009)
New Revision: 10645

Modified:
   gnuradio/trunk/usrp2/host/lib/pktfilter.cc
   gnuradio/trunk/usrp2/host/lib/pktfilter.h
   gnuradio/trunk/usrp2/host/lib/usrp2_impl.cc
Log:
Applied Douglas Geiger's packet filter patch, with cleanup.  Fixes ticket:286.

Modified: gnuradio/trunk/usrp2/host/lib/pktfilter.cc
===================================================================
--- gnuradio/trunk/usrp2/host/lib/pktfilter.cc  2009-03-19 05:43:44 UTC (rev 
10644)
+++ gnuradio/trunk/usrp2/host/lib/pktfilter.cc  2009-03-19 17:09:20 UTC (rev 
10645)
@@ -148,5 +148,40 @@
     
     return pf;
   }
+  /*
+   * Return a filter that harvests inbound packets with the specified 
ethertype and target USRP2 MAC address.
+   * \param ethertype  the ethertype we're looking for
+   * \param usrp_mac    our target USRP2 MAC address
+   */
+  pktfilter *
+  pktfilter::make_ethertype_inbound_target (unsigned short ethertype, const 
unsigned char *usrp_mac)
+  {
+    static const int MAX_LEN = 20;
+    sock_filter        *inst = new sock_filter [MAX_LEN];
+    pktfilter  *pf = new pktfilter ();
+
+    __u16 tmac_hi = (usrp_mac[0] << 8) | usrp_mac[1];
+    __u32 tmac_lo = (usrp_mac[2] << 24) | (usrp_mac[3] << 16) | (usrp_mac[4] 
<< 8) | usrp_mac[5];
+    
+    // ignore packets that have a different ethertype
+    // and only return packets that have a source mac address == usrp_mac
+    
+    int i = 0;
+    inst[i++] = make_stmt (BPF_LD|BPF_H|BPF_ABS, 12);  // load ethertype
+    inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, ethertype, 0, 5);
+    inst[i++] = make_stmt (BPF_LD|BPF_W|BPF_ABS, 8);   // load low 32-bit of 
src mac
+    inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, tmac_lo, 0, 3);
+    inst[i++] = make_stmt (BPF_LD|BPF_H|BPF_ABS, 6);   // load high 16-bits of 
src mac
+    inst[i++] = make_jump (BPF_JMP|BPF_JEQ|BPF_K, tmac_hi, 0, 1);
+    inst[i++] = make_stmt (BPF_RET|BPF_K, (unsigned) -1);      // return whole 
packet
+    inst[i++] = make_stmt (BPF_RET|BPF_K, 0);          // return 0 (ignore 
packet)
+    
+    assert (i <= MAX_LEN);
+    
+    pf->d_inst = inst;
+    pf->d_len = i;
+    
+    return pf;
+  }
   
 } // namespace usrp2

Modified: gnuradio/trunk/usrp2/host/lib/pktfilter.h
===================================================================
--- gnuradio/trunk/usrp2/host/lib/pktfilter.h   2009-03-19 05:43:44 UTC (rev 
10644)
+++ gnuradio/trunk/usrp2/host/lib/pktfilter.h   2009-03-19 17:09:20 UTC (rev 
10645)
@@ -48,6 +48,15 @@
      */
     static pktfilter *make_ethertype_inbound (unsigned short ethertype,
                                              const unsigned char *our_mac);
+
+    /*!
+     * \brief Return a filter that harvests inbound packets with the specified 
ethertype
+     *        and source MAC address
+     * \param ethertype        the ethertype we're looking for
+     * \param usrp_mac         the source MAC address
+     */
+    static pktfilter *make_ethertype_inbound_target (unsigned short ethertype,
+                                                    const unsigned char 
*usrp_mac); 
   };
   
 } // namespace usrp2

Modified: gnuradio/trunk/usrp2/host/lib/usrp2_impl.cc
===================================================================
--- gnuradio/trunk/usrp2/host/lib/usrp2_impl.cc 2009-03-19 05:43:44 UTC (rev 
10644)
+++ gnuradio/trunk/usrp2/host/lib/usrp2_impl.cc 2009-03-19 17:09:20 UTC (rev 
10645)
@@ -138,12 +138,15 @@
     if (!d_eth_buf->open(ifc, htons(U2_ETHERTYPE)))
       throw std::runtime_error("Unable to register USRP2 protocol");
     
-    d_pf = pktfilter::make_ethertype_inbound(U2_ETHERTYPE, d_eth_buf->mac());
+    d_addr = p->addr;
+
+    // Create a packet filter for U2_ETHERTYPE packets sourced from target 
USRP2
+    u2_mac_addr_t usrp_mac;
+    parse_mac_addr(d_addr, &usrp_mac);
+    d_pf = pktfilter::make_ethertype_inbound_target(U2_ETHERTYPE, (const 
unsigned char*)&(usrp_mac.addr));
     if (!d_pf || !d_eth_buf->attach_pktfilter(d_pf))
       throw std::runtime_error("Unable to attach packet filter.");
     
-    d_addr = p->addr;
-    
     if (USRP2_IMPL_DEBUG)
       std::cerr << "usrp2 constructor: using USRP2 at " << d_addr << std::endl;