[Commit-gnuradio] r9015 - usrp2/branches/features/host-ng/host-ng/lib

[eb]

Author: eb
Date: 2008-07-25 13:36:24 -0600 (Fri, 25 Jul 2008)
New Revision: 9015

Modified:
   usrp2/branches/features/host-ng/host-ng/lib/find.cc
Log:
cleaned up usrp2::find

Modified: usrp2/branches/features/host-ng/host-ng/lib/find.cc
===================================================================
--- usrp2/branches/features/host-ng/host-ng/lib/find.cc 2008-07-25 14:59:29 UTC 
(rev 9014)
+++ usrp2/branches/features/host-ng/host-ng/lib/find.cc 2008-07-25 19:36:24 UTC 
(rev 9015)
@@ -23,14 +23,91 @@
 #include <usrp2_eth_packet.h>
 #include <usrp2/usrp2.h>
 #include <boost/scoped_ptr.hpp>
+#include <boost/date_time/posix_time/posix_time_types.hpp>
 #include "ethernet.h"
 #include "pktfilter.h"
 #include <iostream>
+#include <stdexcept>
 
 #define FIND_DEBUG 0
 
+
+// FIXME move to gruel
+
+static struct timeval
+time_duration_to_timeval(boost::posix_time::time_duration delta)
+{
+  long total_us = delta.total_microseconds();
+  if (total_us < 0)
+    throw std::invalid_argument("duration_to_time: delta is negative");
+
+  struct timeval tv;
+  tv.tv_sec =  total_us / 1000000;
+  tv.tv_usec = total_us % 1000000;
+  return tv;
+}
+
+
 namespace usrp2 {
 
+  static props
+  reply_to_props(const op_id_reply_t *r)
+  {
+    const uint8_t *mac = (const uint8_t *)&r->addr;
+    char addr_buf[128];
+    snprintf(addr_buf, sizeof(addr_buf), "%02x:%02x:%02x:%02x:%02x:%02x",
+            mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
+      
+    props p;
+    p.addr = std::string(addr_buf);  
+    p.hw_rev = ntohs(r->hw_rev);
+    memcpy(p.fpga_md5sum, r->fpga_md5sum, sizeof(p.fpga_md5sum));
+    memcpy(p.sw_md5sum, r->sw_md5sum, sizeof(p.sw_md5sum));
+    return p;
+  }
+
+  static void
+  read_replies(ethernet *enet, struct timeval timeout,
+              const std::string &target_addr, props_vector_t &result)
+  {
+    struct reply {
+      u2_eth_packet_t  h;
+      op_id_reply_t    op_id_reply;
+    };
+    
+    uint8_t pktbuf[ethernet::MAX_PKTLEN];
+    memset(pktbuf, 0, sizeof(pktbuf));
+
+    fd_set read_fds;
+    FD_ZERO(&read_fds);
+    FD_SET(enet->fd(), &read_fds);
+    
+    select(enet->fd()+1, &read_fds, 0, 0, &timeout);
+    while(1) {
+      memset(pktbuf, 0, sizeof(pktbuf));
+      int len = enet->read_packet_dont_block(pktbuf, sizeof(pktbuf));
+      if (len < 0){
+       perror("usrp2_basic: read_packet_dont_block");
+        return;
+      }
+      if (len == 0)
+       break;
+      
+      reply *rp = (reply *)pktbuf;
+      if (u2p_chan(&rp->h.fixed) != CONTROL_CHAN)      // ignore
+       continue;
+      if (rp->op_id_reply.opcode != OP_ID_REPLY)       // ignore
+       continue;
+      
+      props p = reply_to_props(&rp->op_id_reply);
+      if (FIND_DEBUG)
+       std::cerr << "usrp2::find: response from " << p.addr << std::endl;
+      
+      if ((target_addr == "") || (target_addr == p.addr))
+       result.push_back(p);
+    }
+  }
+
   props_vector_t
   find(const std::string &ifc, const std::string &addr)
   {
@@ -46,12 +123,6 @@
       op_generic_t     op_id;
     };
     
-    struct reply {
-      u2_eth_packet_t  h;
-      op_id_reply_t    op_id_reply;
-    };
-    
-    std::vector<op_id_reply_t> replies;
     std::auto_ptr<ethernet> enet(new ethernet()); 
     
     if (!enet->open(ifc, htons(U2_ETHERTYPE)))
@@ -86,50 +157,22 @@
       std::cerr << "usrp2::find: broadcast ID command" << std::endl;
     
     /*
-     * Wait no longer than 10ms and read all packets available.
+     * Gather all responses that occur within 50ms
      */
-    fd_set read_fds;
-    FD_ZERO(&read_fds);
-    FD_SET(enet->fd(), &read_fds);
-    struct timeval timeout;
-    timeout.tv_sec = 0;
-    timeout.tv_usec = 10 * 1000;       // 10 ms
-    
-    select(enet->fd()+1, &read_fds, 0, 0, &timeout);
-    while(1) {
-      memset(pktbuf, 0, sizeof(pktbuf));
-      len = enet->read_packet_dont_block(pktbuf, sizeof(pktbuf));
-      if (len < 0){
-       perror("usrp2_basic: read_packet_dont_block");
-        return result;
-      }
-      if (len == 0)
+    boost::posix_time::ptime 
start(boost::posix_time::microsec_clock::universal_time());
+    boost::posix_time::ptime limit(start + 
boost::posix_time::milliseconds(50));
+    boost::posix_time::ptime now;
+
+    while (1){
+      now = boost::posix_time::microsec_clock::universal_time();
+      if (now >= limit)
        break;
-      
-      reply *rp = (reply *)pktbuf;
-      if (u2p_chan(&rp->h.fixed) != CONTROL_CHAN)  // ignore
-       continue;
-      if (rp->op_id_reply.opcode != OP_ID_REPLY)       // ignore
-       continue;
-      
-      const uint8_t *mac = (const uint8_t *)&rp->op_id_reply.addr;
-      char addr_buf[128];
-      snprintf(addr_buf, sizeof(addr_buf), "%02x:%02x:%02x:%02x:%02x:%02x",
-              mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
-      
-      props p;
-      p.addr = std::string(addr_buf);  
-      p.hw_rev = ntohs(rp->op_id_reply.hw_rev);
-      memcpy(p.fpga_md5sum, rp->op_id_reply.fpga_md5sum, 
sizeof(p.fpga_md5sum));
-      memcpy(p.sw_md5sum, rp->op_id_reply.sw_md5sum, sizeof(p.sw_md5sum));
-      
-      if (FIND_DEBUG)
-       std::cerr << "usrp2::find: response from " << p.addr << std::endl;
-      
-      if ((addr == "") || (addr == p.addr))
-       result.push_back(p);
+
+      boost::posix_time::time_duration delta(limit - now);
+      struct timeval timeout = time_duration_to_timeval(delta);
+
+      read_replies(enet.get(), timeout, addr, result);
     }
-    
     return result;
   }