Applied Douglas Geiger's packet filter patch, with cleanup. Fixes ticket:286.

git-svn-id: http://gnuradio.org/svn/gnuradio/trunk@10645 221aa14e-8319-0410-a670-987f0aec2ac5

diff --git a/usrp2/host/lib/pktfilter.cc b/usrp2/host/lib/pktfilter.cc
index 8341d015b909e978227ef0c44266d3ab534c270a..1ccf904465c0c7b8caf6ab3ac561eb03cf88833d 100644 (file)
--- a/usrp2/host/lib/pktfilter.cc
+++ b/usrp2/host/lib/pktfilter.cc
@@ -148,5 +148,40 @@ namespace usrp2 {
     
     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

diff --git a/usrp2/host/lib/pktfilter.h b/usrp2/host/lib/pktfilter.h
index 8b07fe1488f1b248c5a1f6e9002c57424974df38..09937ae34e7eb3c1a8b49e4c7b0dbdd69a11f776 100644 (file)
--- a/usrp2/host/lib/pktfilter.h
+++ b/usrp2/host/lib/pktfilter.h
@@ -48,6 +48,15 @@ namespace usrp2 {
      */
     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

diff --git a/usrp2/host/lib/usrp2_impl.cc b/usrp2/host/lib/usrp2_impl.cc
index adbfd6d69025744203fb1c1411465a9645c33d73..98c3eb7cfdaef7846819e33c669fa18fdbc2012d 100644 (file)
--- a/usrp2/host/lib/usrp2_impl.cc
+++ b/usrp2/host/lib/usrp2_impl.cc
@@ -138,12 +138,15 @@ namespace usrp2 {
     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;