diff options
Diffstat (limited to 'usrp2/host/lib/find.cc')
| -rw-r--r-- | usrp2/host/lib/find.cc | 181 |
1 files changed, 181 insertions, 0 deletions
diff --git a/usrp2/host/lib/find.cc b/usrp2/host/lib/find.cc new file mode 100644 index 000000000..f6c04a58e --- /dev/null +++ b/usrp2/host/lib/find.cc @@ -0,0 +1,181 @@ +/* -*- c++ -*- */ +/* + * Copyright 2008 Free Software Foundation, Inc. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +#ifdef HAVE_CONFIG_H +#include <config.h> +#endif + +#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 <string.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) + { + if (FIND_DEBUG) { + std::cerr << "usrp2::find: Searching interface " << ifc << " for " + << (addr == "" ? "all USRP2s" : addr) + << std::endl; + } + + props_vector_t result; + struct command { + u2_eth_packet_t h; + op_generic_t op_id; + }; + + std::auto_ptr<ethernet> enet(new ethernet()); + + if (!enet->open(ifc, htons(U2_ETHERTYPE))) + return result; + + std::auto_ptr<pktfilter> pf(pktfilter::make_ethertype_inbound(U2_ETHERTYPE, enet->mac())); + if (!enet->attach_pktfilter(pf.get())) + return result; + + static u2_mac_addr_t broadcast_mac_addr = + {{ 0xff, 0xff, 0xff, 0xff, 0xff, 0xff }}; + + uint8_t pktbuf[ethernet::MAX_PKTLEN]; + memset(pktbuf, 0, sizeof(pktbuf)); + + command *c = (command *)pktbuf; + c->h.ehdr.ethertype = htons(U2_ETHERTYPE); + c->h.ehdr.dst = broadcast_mac_addr; + memcpy(&c->h.ehdr.src, enet->mac(), 6); + c->h.thdr.flags = 0; + c->h.thdr.seqno = 0; + c->h.thdr.ack = 0; + u2p_set_word0(&c->h.fixed, 0, CONTROL_CHAN); + u2p_set_timestamp(&c->h.fixed, -1); + c->op_id.opcode = OP_ID; + c->op_id.len = sizeof(c->op_id); + int len = std::max((size_t) ethernet::MIN_PKTLEN, sizeof(command)); + if (enet->write_packet(c, len) != len) + return result; + + if (FIND_DEBUG) + std::cerr << "usrp2::find: broadcast ID command" << std::endl; + + /* + * Gather all responses that occur within 50ms + */ + 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; + + 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; + } + +} // namespace usrp2 + |