3 files changed, 50 insertions, 3 deletions

diff --git a/usrp2/host/lib/pktfilter.cc b/usrp2/host/lib/pktfilter.cc
index 8341d015b..1ccf90446 100644
--- 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 8b07fe148..09937ae34 100644
--- 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 adbfd6d69..98c3eb7cf 100644
--- 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;