git.gag.com Git - debian/gnuradio/blob - usrp2/host/lib/find.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 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 <usrp2_eth_packet.h>
  24 #include <usrp2/usrp2.h>
  25 #include <boost/scoped_ptr.hpp>
  26 #include <boost/date_time/posix_time/posix_time_types.hpp>
  27 #include "ethernet.h"
  28 #include "pktfilter.h"
  29 #include <string.h>
  30 #include <iostream>
  31 #include <stdexcept>
  32 #include <cstdio>
  33
  34 #define FIND_DEBUG 0
  35
  36
  37 // FIXME move to gruel
  38
  39 static struct timeval
  40 time_duration_to_timeval(boost::posix_time::time_duration delta)
  41 {
  42   long total_us = delta.total_microseconds();
  43   if (total_us < 0)
  44     throw std::invalid_argument("duration_to_time: delta is negative");
  45
  46   struct timeval tv;
  47   tv.tv_sec =  total_us / 1000000;
  48   tv.tv_usec = total_us % 1000000;
  49   return tv;
  50 }
  51
  52
  53 namespace usrp2 {
  54
  55   static props
  56   reply_to_props(const op_id_reply_t *r)
  57   {
  58     const uint8_t *mac = (const uint8_t *)&r->addr;
  59     char addr_buf[128];
  60     snprintf(addr_buf, sizeof(addr_buf), "%02x:%02x:%02x:%02x:%02x:%02x",
  61              mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
  62
  63     props p;
  64     p.addr = std::string(addr_buf);
  65     p.hw_rev = ntohs(r->hw_rev);
  66     memcpy(p.fpga_md5sum, r->fpga_md5sum, sizeof(p.fpga_md5sum));
  67     memcpy(p.sw_md5sum, r->sw_md5sum, sizeof(p.sw_md5sum));
  68     return p;
  69   }
  70
  71   static void
  72   read_replies(ethernet *enet, struct timeval timeout,
  73                const std::string &target_addr, props_vector_t &result)
  74   {
  75     struct reply {
  76       u2_eth_packet_t   h;
  77       op_id_reply_t     op_id_reply;
  78     };
  79
  80     uint8_t pktbuf[ethernet::MAX_PKTLEN];
  81     memset(pktbuf, 0, sizeof(pktbuf));
  82
  83     fd_set read_fds;
  84     FD_ZERO(&read_fds);
  85     FD_SET(enet->fd(), &read_fds);
  86
  87     select(enet->fd()+1, &read_fds, 0, 0, &timeout);
  88     while(1) {
  89       memset(pktbuf, 0, sizeof(pktbuf));
  90       int len = enet->read_packet_dont_block(pktbuf, sizeof(pktbuf));
  91       if (len < 0){
  92         perror("usrp2_basic: read_packet_dont_block");
  93         return;
  94       }
  95       if (len == 0)
  96         break;
  97
  98       reply *rp = (reply *)pktbuf;
  99       if (u2p_chan(&rp->h.fixed) != CONTROL_CHAN)       // ignore
 100         continue;
 101       if (rp->op_id_reply.opcode != OP_ID_REPLY)        // ignore
 102         continue;
 103
 104       props p = reply_to_props(&rp->op_id_reply);
 105       if (FIND_DEBUG)
 106         std::cerr << "usrp2::find: response from " << p.addr << std::endl;
 107
 108       if ((target_addr == "") || (target_addr == p.addr))
 109         result.push_back(p);
 110     }
 111   }
 112
 113   props_vector_t
 114   find(const std::string &ifc, const std::string &addr)
 115   {
 116     if (FIND_DEBUG) {
 117       std::cerr << "usrp2::find: Searching interface " << ifc << " for "
 118                 << (addr == "" ? "all USRP2s" : addr)
 119                 << std::endl;
 120     }
 121
 122     props_vector_t result;
 123     struct command {
 124       u2_eth_packet_t   h;
 125       op_generic_t      op_id;
 126     };
 127
 128     std::auto_ptr<ethernet> enet(new ethernet());
 129
 130     if (!enet->open(ifc, htons(U2_ETHERTYPE)))
 131       return result;
 132
 133     std::auto_ptr<pktfilter> pf(pktfilter::make_ethertype_inbound(U2_ETHERTYPE, enet->mac()));
 134     if (!enet->attach_pktfilter(pf.get()))
 135       return result;
 136
 137     static u2_mac_addr_t broadcast_mac_addr =
 138       {{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }};
 139
 140     uint8_t pktbuf[ethernet::MAX_PKTLEN];
 141     memset(pktbuf, 0, sizeof(pktbuf));
 142
 143     command *c = (command *)pktbuf;
 144     c->h.ehdr.ethertype = htons(U2_ETHERTYPE);
 145     c->h.ehdr.dst = broadcast_mac_addr;
 146     memcpy(&c->h.ehdr.src, enet->mac(), 6);
 147     c->h.thdr.flags = 0;
 148     c->h.thdr.seqno = 0;
 149     c->h.thdr.ack = 0;
 150     u2p_set_word0(&c->h.fixed, 0, CONTROL_CHAN);
 151     u2p_set_timestamp(&c->h.fixed, -1);
 152     c->op_id.opcode = OP_ID;
 153     c->op_id.len = sizeof(c->op_id);
 154     int len = std::max((size_t) ethernet::MIN_PKTLEN, sizeof(command));
 155     if (enet->write_packet(c, len) != len)
 156       return result;
 157
 158     if (FIND_DEBUG)
 159       std::cerr << "usrp2::find: broadcast ID command" << std::endl;
 160
 161     /*
 162      * Gather all responses that occur within 50ms
 163      */
 164     boost::posix_time::ptime start(boost::posix_time::microsec_clock::universal_time());
 165     boost::posix_time::ptime limit(start + boost::posix_time::milliseconds(50));
 166     boost::posix_time::ptime now;
 167
 168     while (1){
 169       now = boost::posix_time::microsec_clock::universal_time();
 170       if (now >= limit)
 171         break;
 172
 173       boost::posix_time::time_duration delta(limit - now);
 174       struct timeval timeout = time_duration_to_timeval(delta);
 175
 176       read_replies(enet.get(), timeout, addr, result);
 177     }
 178     return result;
 179   }
 180
 181 } // namespace usrp2
 182