mpmd: Let 'find' via broadcast check for reachability

In particular, when running uhd_find_devices, this will limit the
devices to ones that can actually be reached via CHDR. There is a new
key, find_all, which allows finding all devices even those not
reachable from UHD.

Reviewed-by: Brent Stapleton <brent.stapleton@ettus.com>

4 files changed, 198 insertions, 6 deletions

diff --git a/host/docs/usrp_n3xx.dox b/host/docs/usrp_n3xx.dox
index 9260f2ea5..8d37d9fa3 100644
--- a/host/docs/usrp_n3xx.dox
+++ b/host/docs/usrp_n3xx.dox
@@ -298,6 +298,7 @@ For a list of which arguments can be passed into make(), see Section
  addr                | IPv4 address of primary SFP+ port to connect to.                             | All N3xx          | addr=192.168.30.2
  second_addr         | IPv4 address of secondary SFP+ port to connect to.                           | All N3xx          | second_addr=192.168.40.2
  mgmt_addr           | IPv4 address or hostname which to connect the RPC client. Defaults to `addr'.| All N3xx          | mgmt_addr=ni-sulfur-311FE00 (can also go to RJ45)
+ find_all            | When using broadcast, find all devices, even if unreachable via CHDR.        | All N3xx          | find_all=1
  master_clock_rate   | Master Clock Rate in Hz                                                      | N310              | master_clock_rate=125e6
  identify            | Causes front-panel LEDs to blink. The duration is variable.                  | N310              | identify=5 (will blink for about 5 seconds)
  serialize_init      | Force serial initialization of daughterboards.                               | All N3xx          | serialize_init=1
diff --git a/host/lib/usrp/mpmd/mpmd_impl.cpp b/host/lib/usrp/mpmd/mpmd_impl.cpp
index ae0e31f62..241c834cd 100644
--- a/host/lib/usrp/mpmd/mpmd_impl.cpp
+++ b/host/lib/usrp/mpmd/mpmd_impl.cpp
@@ -32,6 +32,8 @@
 using namespace uhd;
 using namespace uhd::mpmd;
 
+constexpr char mpmd_impl::MPM_DISC_RESPONSE_PREAMBLE[];
+
 namespace {
     /*************************************************************************
      * Local constants
@@ -45,6 +47,9 @@ namespace {
     const std::vector<size_t> MPM_COMPAT_NUM = {1, 1};
     //! Timeout value for the update_component RPC call (ms)
     const size_t MPMD_UPDATE_COMPONENT_TIMEOUT     = 20000;
+    constexpr char MPMD_CHDR_REACHABILITY_KEY[] = "reachable";
+    constexpr char MPMD_CHDR_REACHABILITY_NEGATIVE[] = "No";
+
 
     /*************************************************************************
      * Helper functions
@@ -376,12 +381,21 @@ namespace {
             UHD_LOG_WARNING("MPMD", err_msg);
         }
     }
+
+    device_addr_t flag_dev_as_unreachable(const device_addr_t& device_args)
+    {
+        device_addr_t flagged_device_args(device_args);
+        flagged_device_args[MPMD_CHDR_REACHABILITY_KEY] =
+            MPMD_CHDR_REACHABILITY_NEGATIVE;
+        return flagged_device_args;
+    }
 }
 
 
 /*****************************************************************************
  * Static class attributes
  ****************************************************************************/
+const std::string mpmd_impl::MPM_FINDALL_KEY = "find_all";
 const size_t mpmd_impl::MPM_DISCOVERY_PORT = 49600;
 const std::string mpmd_impl::MPM_DISCOVERY_PORT_KEY = "discovery_port";
 const size_t mpmd_impl::MPM_RPC_PORT = 49601;
@@ -593,8 +607,10 @@ void mpmd_impl::setup_rpc_blocks(
 /*****************************************************************************
  * Find, Factory & Registry
  ****************************************************************************/
-device_addrs_t mpmd_find_with_addr(const std::string& mgmt_addr, const device_addr_t& hint_)
-{
+device_addrs_t mpmd_find_with_addr(
+        const std::string& mgmt_addr,
+        const device_addr_t& hint_
+) {
     UHD_ASSERT_THROW(not mgmt_addr.empty());
     const std::string mpm_discovery_port = hint_.get(
         mpmd_impl::MPM_DISCOVERY_PORT_KEY,
@@ -633,7 +649,7 @@ device_addrs_t mpmd_find_with_addr(const std::string& mgmt_addr, const device_ad
         }
         // Verify we didn't receive something other than an MPM discovery
         // response
-        if (result[0] != "USRP-MPM") {
+        if (result[0] != mpmd_impl::MPM_DISC_RESPONSE_PREAMBLE) {
             continue;
         }
         const std::string recv_addr = comm->get_recv_addr();
@@ -664,7 +680,9 @@ device_addrs_t mpmd_find_with_addr(const std::string& mgmt_addr, const device_ad
             boost::algorithm::split(value, el,
                                     [](const char& in) { return in == '='; },
                                     boost::token_compress_on);
-            new_addr[value[0]] = value[1];
+            if (value[0] != xport::MGMT_ADDR_KEY) {
+                new_addr[value[0]] = value[1];
+            }
         }
         // filter the discovered device below by matching optional keys
         if (
@@ -762,6 +780,9 @@ device_addrs_t mpmd_find(const device_addr_t& hint_)
     if (not hints.empty() and
             (hints[0].has_key(xport::FIRST_ADDR_KEY) or
              hints[0].has_key(xport::MGMT_ADDR_KEY))) {
+        // Note: We don't try and connect to the devices in this mode, because
+        // we only get here if the user specified addresses, and we assume she
+        // knows what she's doing.
         return mpmd_find_with_addrs(hints);
     }
 
@@ -769,7 +790,37 @@ device_addrs_t mpmd_find(const device_addr_t& hint_)
     if (hints.empty()) {
         hints.resize(1);
     }
-    return mpmd_find_with_bcast(hints[0]);
+    const auto bcast_mpm_devs = mpmd_find_with_bcast(hints[0]);
+    UHD_LOG_TRACE("MPMD FIND",
+        "Found " << bcast_mpm_devs.size() << " device via broadcast.");
+    const bool find_all = hint_.has_key(mpmd_impl::MPM_FINDALL_KEY);
+    if (find_all) {
+        UHD_LOG_TRACE("MPMD FIND",
+            "User provided " << mpmd_impl::MPM_FINDALL_KEY << ", will not "
+            "filter devices based on CHDR accessibility.");
+    }
+    // Filter found devices for those that we can actually talk to via CHDR
+    device_addrs_t filtered_mpm_devs;
+    for (const auto &mpm_dev : bcast_mpm_devs) {
+        const auto reachable_device_addr =
+            mpmd_mboard_impl::is_device_reachable(mpm_dev);
+        if (bool(reachable_device_addr)) {
+            filtered_mpm_devs.push_back(reachable_device_addr.get());
+        } else if (find_all) {
+            filtered_mpm_devs.emplace_back(
+                flag_dev_as_unreachable(mpm_dev)
+            );
+        }
+    }
+
+    if (filtered_mpm_devs.empty() and not bcast_mpm_devs.empty()) {
+        UHD_LOG_INFO("MPMD FIND",
+            "Found MPM devices, but none are reachable for a UHD session. "
+            "Specify " << mpmd_impl::MPM_FINDALL_KEY << " to find all devices."
+        );
+    }
+
+    return filtered_mpm_devs;
 }
 
 static device::sptr mpmd_make(const device_addr_t& device_args)
diff --git a/host/lib/usrp/mpmd/mpmd_impl.hpp b/host/lib/usrp/mpmd/mpmd_impl.hpp
index f24f6d3fc..c935a8b8e 100644
--- a/host/lib/usrp/mpmd/mpmd_impl.hpp
+++ b/host/lib/usrp/mpmd/mpmd_impl.hpp
@@ -15,6 +15,7 @@
 #include <uhd/types/dict.hpp>
 #include <uhd/utils/tasks.hpp>
 #include <uhd/transport/muxed_zero_copy_if.hpp>
+#include <boost/optional.hpp>
 #include <map>
 #include <memory>
 
@@ -29,6 +30,16 @@ class mpmd_mboard_impl
     using uptr = std::unique_ptr<mpmd_mboard_impl>;
     using dev_info = std::map<std::string, std::string>;
 
+    /*** Static helper *******************************************************/
+    /*! Will run some checks to determine if this device can be reached from
+     *  the current UHD session
+     *
+     *  \param device_addr Device args. Must contain an mgmt_addr.
+     */
+    static boost::optional<device_addr_t> is_device_reachable(
+        const device_addr_t& device_addr
+    );
+
     /*** Structors ***********************************************************/
     /*! Ctor: Claim device or throw an exception on failure.
      *
@@ -159,6 +170,9 @@ class mpmd_mboard_impl
 class mpmd_impl : public uhd::usrp::device3_impl
 {
 public:
+    //! Device arg key which will allow finding all devices, even those not
+    // reachable via CHDR.
+    static const std::string MPM_FINDALL_KEY;
     //! Port on which the discovery process is listening (default value, it is
     //  user-overridable)
     static const size_t MPM_DISCOVERY_PORT;
@@ -177,6 +191,9 @@ public:
     static const std::string MPM_ECHO_CMD;
     //! This is the RPC command that will return the last known error from MPM.
     static const std::string MPM_RPC_GET_LAST_ERROR_CMD;
+    //! The preamble for any response on the discovery port. Can be used to
+    //  verify that the response is actually an MPM device.
+    static constexpr char MPM_DISC_RESPONSE_PREAMBLE[] = "USRP-MPM";
 
     /**************************************************************************
      * Structors
diff --git a/host/lib/usrp/mpmd/mpmd_mboard_impl.cpp b/host/lib/usrp/mpmd/mpmd_mboard_impl.cpp
index 274e2d370..6df78a9dd 100644
--- a/host/lib/usrp/mpmd/mpmd_mboard_impl.cpp
+++ b/host/lib/usrp/mpmd/mpmd_mboard_impl.cpp
@@ -7,6 +7,7 @@
 #include "mpmd_impl.hpp"
 #include <uhd/utils/safe_call.hpp>
 #include <uhd/utils/log.hpp>
+#include <uhd/transport/udp_simple.hpp>
 #include <chrono>
 #include <thread>
 
@@ -19,7 +20,12 @@ namespace {
     //! Default timeout value for the init() RPC call (ms)
     const size_t MPMD_DEFAULT_INIT_TIMEOUT    = 120000;
     //! Default timeout value for RPC calls (ms)
-    const size_t MPMD_DEFAULT_RPC_TIMEOUT     = 2000;
+    constexpr size_t MPMD_DEFAULT_RPC_TIMEOUT     = 2000;
+    //! Short timeout value for RPC calls (ms), used for calls that shouldn't
+    // take long. This value can be used to quickly determine a link status.
+    constexpr size_t MPMD_SHORT_RPC_TIMEOUT     = 2000;
+    //! Timeout for pings (seconds).
+    constexpr double MPMD_PING_TIMEOUT          = 0.1;
     //! Default session ID (MPM will recognize a session by this name)
     const std::string MPMD_DEFAULT_SESSION_ID = "UHD";
     //! Key to initialize a ping/measurement latency test
@@ -33,6 +39,47 @@ namespace {
     /*************************************************************************
      * Helper functions
      ************************************************************************/
+    /*! Return true if we can MPM ping a device via discovery service.
+     */
+    bool is_pingable(const std::string& ip_addr, const std::string& udp_port)
+    {
+        auto udp = uhd::transport::udp_simple::make_broadcast(
+            ip_addr,
+            udp_port
+        );
+        const std::string send_buf(
+            uhd::mpmd::mpmd_impl::MPM_ECHO_CMD + " ping"
+        );
+        std::vector<uint8_t> recv_buf;
+        recv_buf.resize(send_buf.size(), ' ');
+        udp->send(boost::asio::buffer(send_buf.c_str(), send_buf.size()));
+        const size_t len =
+            udp->recv(boost::asio::buffer(recv_buf), MPMD_PING_TIMEOUT);
+        if (len == 0) {
+            UHD_LOG_TRACE("MPMD",
+                "Received no MPM ping, assuming device is unreachable.");
+            return false;
+        }
+        if (len != send_buf.size()) {
+            UHD_LOG_DEBUG("MPMD",
+                "Received bad return packet on MPM ping. Assuming endpoint"
+                " is not a valid MPM device.");
+            return false;
+        }
+        if (std::memcmp(
+                    (void *) &recv_buf[0],
+                    (void *) &send_buf[0],
+                    send_buf.size()) != 0) {
+            UHD_LOG_DEBUG("MPMD",
+                "Received invalid return packet on MPM ping. Assuming endpoint"
+                " is not a valid MPM device.");
+            return false;
+        }
+        return true;
+    }
+
+    /*! Call init() on an MPM device.
+     */
     void init_device(
             uhd::rpc_client::sptr rpc,
             const uhd::device_addr_t mb_args
@@ -162,6 +209,82 @@ namespace {
 using namespace uhd;
 using namespace uhd::mpmd;
 
+/******************************************************************************
+ * Static Helpers
+ *****************************************************************************/
+boost::optional<device_addr_t> mpmd_mboard_impl::is_device_reachable(
+        const device_addr_t &device_addr
+) {
+    UHD_LOG_TRACE("MPMD",
+        "Checking accessibility of device `" << device_addr.to_string()
+        << "'");
+    UHD_ASSERT_THROW(device_addr.has_key(xport::MGMT_ADDR_KEY));
+    const std::string rpc_addr = device_addr.get(xport::MGMT_ADDR_KEY);
+    const size_t rpc_port = device_addr.cast<size_t>(
+        mpmd_impl::MPM_RPC_PORT_KEY,
+        mpmd_impl::MPM_RPC_PORT
+    );
+    auto rpcc = uhd::rpc_client::make(rpc_addr, rpc_port);
+    rpcc->set_timeout(MPMD_SHORT_RPC_TIMEOUT);
+    // 1) Read back device info
+    dev_info device_info_dict;
+    try {
+        device_info_dict = rpcc->request<dev_info>("get_device_info");
+    } catch (const uhd::runtime_error& e) {
+        UHD_LOG_ERROR("MPMD", e.what());
+    } catch (...) {
+        UHD_LOG_DEBUG("MPMD",
+            "Unexpected exception when trying to query device info. Flagging "
+            "device as unreachable.");
+        return boost::optional<device_addr_t>();
+    }
+    // 2) Check for local device
+    if (device_info_dict.count("connection") and
+            device_info_dict.at("connection") == "local") {
+        UHD_LOG_TRACE("MPMD", "Device is local, flagging as reachable.");
+        return boost::optional<device_addr_t>(device_addr);
+    }
+    // 3) Check for network-reachable device
+    // Note: This makes the assumption that devices will always allow RPC
+    // connections on their CHDR addresses.
+    const std::vector<std::string> addr_keys = {"second_addr", "addr"};
+    for (const auto& addr_key : addr_keys) {
+        if (not device_info_dict.count(addr_key)) {
+            continue;
+        }
+        const std::string chdr_addr = device_info_dict.at(addr_key);
+        UHD_LOG_TRACE("MPMD",
+            "Checking reachability via network addr " << chdr_addr);
+        try {
+            // First do an MPM ping -- there is some issue with rpclib that can
+            // lead to indefinite timeouts
+            const std::string mpm_discovery_port = device_addr.get(
+                mpmd_impl::MPM_DISCOVERY_PORT_KEY,
+                std::to_string(mpmd_impl::MPM_DISCOVERY_PORT)
+            );
+            if (!is_pingable(chdr_addr, mpm_discovery_port)) {
+                UHD_LOG_TRACE("MPMD",
+                    "Cannot MPM ping, assuming device is unreachable.");
+                continue;
+            }
+            UHD_LOG_TRACE("MPMD",
+                "Was able to ping device, trying RPC connection.");
+            auto chdr_rpcc = uhd::rpc_client::make(chdr_addr, rpc_port);
+            chdr_rpcc->set_timeout(MPMD_SHORT_RPC_TIMEOUT);
+            chdr_rpcc->request<dev_info>("get_device_info");
+            auto device_addr_copy{device_addr};
+            device_addr_copy["addr"] = chdr_addr;
+            return boost::optional<device_addr_t>(device_addr_copy);
+        } catch (...) {
+            UHD_LOG_DEBUG("MPMD",
+                "Failed to reach device on network addr " << chdr_addr << ".");
+        }
+    }
+    // If everything fails, we probably can't talk to this chap.
+    UHD_LOG_TRACE("MPMD",
+        "All reachability checks failed -- assuming device is not reachable.");
+    return boost::optional<device_addr_t>();
+}
 
 /*****************************************************************************
  * Structors