uhd: rfnoc: add async message handler

7 files changed, 421 insertions, 9 deletions

diff --git a/host/lib/include/uhdlib/rfnoc/async_msg.hpp b/host/lib/include/uhdlib/rfnoc/async_msg.hpp
new file mode 100644
index 000000000..8abdb27b8
--- /dev/null
+++ b/host/lib/include/uhdlib/rfnoc/async_msg.hpp
@@ -0,0 +1,85 @@
+//
+// Copyright 2018 Ettus Research, a National Instruments Company
+//
+// SPDX-License-Identifier: GPL-3.0-or-later
+//
+
+#ifndef INCLUDED_UHD_RFNOC_ASYNC_MSG_HPP
+#define INCLUDED_UHD_RFNOC_ASYNC_MSG_HPP
+
+#include <uhd/config.hpp>
+#include <uhd/types/time_spec.hpp>
+#include <uhd/types/sid.hpp>
+#include <vector>
+
+namespace uhd { namespace rfnoc {
+
+    /*!
+     * Async message.
+     */
+    struct async_msg_t
+    {
+        //! Has time?
+        bool has_time_spec;
+
+        //! When the async event occurred.
+        time_spec_t time_spec;
+
+        /*!
+         * The type of event for a receive async message call.
+         */
+        enum event_code_t {
+            //! Nothing happened.
+            EVENT_CODE_NONE = 0x00,
+            //! A burst was successfully transmitted.
+            EVENT_CODE_BURST_ACK  = 0x1,
+            //! An internal send buffer has emptied.
+            EVENT_CODE_UNDERFLOW  = 0x2,
+            //! Same. We use the terms 'underrun' and 'underflow' interchangeably.
+            EVENT_CODE_UNDERRUN  = EVENT_CODE_UNDERFLOW,
+            //! Packet loss or reordering between source and destination,
+            //  at start of burst (i.e. the first packet after an EOB packet
+            //  had the wrong sequence number).
+            EVENT_CODE_SEQ_ERROR  = 0x4,
+            //! Like EVENT_CODE_SEQ_ERROR, but within a burst (i.e., any packet
+            //  other than the first packet had an error)
+            EVENT_CODE_SEQ_ERROR_IN_BURST  = 0x20,
+            //! Data packet had time that was late.
+            EVENT_CODE_LATE_DATA_ERROR = 0x8,
+            //! Command packet had time that was late.
+            EVENT_CODE_LATE_CMD_ERROR = 0x8,
+            //! Packet is carrying arbitrary payload
+            EVENT_CODE_USER_PAYLOAD = 0x40,
+
+            // TODO: For now, we combine legacy TX and RX messages.
+            EVENT_CODE_OVERFLOW     = 0x8,
+            EVENT_CODE_OVERRUN     = EVENT_CODE_OVERFLOW,
+            //! Multi-channel alignment failed.
+            EVENT_CODE_ALIGNMENT    = 0xc,
+            //! The packet could not be parsed.
+            EVENT_CODE_BAD_PACKET   = 0xf
+        } event_code;
+
+        /*!
+         * A special payload populated by custom FPGA fabric.
+         */
+        std::vector<uint32_t> payload;
+
+        //! The SID on the async message packet
+        uint32_t sid;
+
+        async_msg_t(const size_t payload_size=4) :
+              has_time_spec(false),
+              time_spec(0.0),
+              event_code(EVENT_CODE_NONE),
+              payload(payload_size, 0),
+              sid(0)
+        {}
+        //! Return the the id of src block that throw eror
+        uint32_t get_error_src() const { return sid_t(sid).get_src_endpoint(); }
+    };
+
+    }
+}
+#endif /* INCLUDED_UHD_RFNOC_ASYNC_MSG_HPP */
+
diff --git a/host/lib/include/uhdlib/rfnoc/async_msg_handler.hpp b/host/lib/include/uhdlib/rfnoc/async_msg_handler.hpp
new file mode 100644
index 000000000..1f67d05a1
--- /dev/null
+++ b/host/lib/include/uhdlib/rfnoc/async_msg_handler.hpp
@@ -0,0 +1,86 @@
+//
+// Copyright 2016 Ettus Research LLC
+// Copyright 2018 Ettus Research, a National Instruments Company
+//
+// SPDX-License-Identifier: GPL-3.0-or-later
+//
+
+#ifndef INCLUDED_LIBUHD_RFNOC_AYNC_MSG_HANDLER_HPP
+#define INCLUDED_LIBUHD_RFNOC_AYNC_MSG_HANDLER_HPP
+
+#include <uhd/rfnoc/graph.hpp>
+#include <uhd/transport/zero_copy.hpp>
+#include <uhd/types/sid.hpp>
+#include <uhd/types/endianness.hpp>
+#include <uhdlib/rfnoc/async_msg.hpp>
+#include <boost/noncopyable.hpp>
+#include <functional>
+
+namespace uhd { namespace rfnoc {
+
+/*! Async message handler for a uhd::rfnoc::graph
+ *
+ */
+class async_msg_handler : boost::noncopyable
+{
+public:
+    typedef boost::shared_ptr<async_msg_handler> sptr;
+    typedef std::function<void(const async_msg_t&)> async_handler_type;
+
+    /*!
+     * \param recv A transport on which async messages are received
+     * \param send A transport on which to send response messages
+     * \param sid The source part of this is taken as the local address of the
+     *            transports. The remote part is ignored.
+     */
+    static sptr make(
+        uhd::transport::zero_copy_if::sptr recv,
+        uhd::transport::zero_copy_if::sptr send,
+        uhd::sid_t sid,
+        uhd::endianness_t endianness
+    );
+
+    /*! Register an event handler.
+     *
+     * When any message is received with the given event code,
+     * \p handler is called with the async message data as an argument.
+     *
+     * Note that \p handler is called if a message includes a certain event
+     * code, but it does not have to be exclusive. Example: If there are two
+     * event handlers registered, one for EVENT_CODE_OVERRUN and one for
+     * EVENT_CODE_BAD_PACKET, and a message includes both those event codes,
+     * then both event handlers are called.
+     *
+     * Multiple handlers per event code may be registered. The order they are
+     * called in is non-deterministic.
+     *
+     * \returns The number of event handlers registered for this event code.
+     *          Should never return anything less than 1.
+     */
+    virtual int register_event_handler(
+            const async_msg_t::event_code_t event_code,
+            async_handler_type handler
+    ) = 0;
+
+    /*! Post async messages into this message handler.
+     *
+     * This is the entry point for all async messages. When a message
+     * is posted here, the following actions take place:
+     * - If applicable, an event handler is called with \p metadata as the
+     *   argument
+     * - Some messages print error codes (e.g. O, U, L, S)
+     */
+    virtual void post_async_msg(
+            const async_msg_t &metadata
+    ) = 0;
+
+    /*! Return the 16-bit address of this async message
+    */
+    virtual uint32_t get_local_addr() const = 0;
+};
+
+
+}}; /* namespace uhd::rfnoc */
+
+#endif /* INCLUDED_LIBUHD_RFNOC_AYNC_MSG_HANDLER_HPP */
+// vim: sw=4 et:
diff --git a/host/lib/include/uhdlib/rfnoc/graph_impl.hpp b/host/lib/include/uhdlib/rfnoc/graph_impl.hpp
index 404369618..8da88b94f 100644
--- a/host/lib/include/uhdlib/rfnoc/graph_impl.hpp
+++ b/host/lib/include/uhdlib/rfnoc/graph_impl.hpp
@@ -8,6 +8,7 @@
 #ifndef INCLUDED_LIBUHD_RFNOC_GRAPH_IMPL_HPP
 #define INCLUDED_LIBUHD_RFNOC_GRAPH_IMPL_HPP
 
+#include "async_msg_handler.hpp"
 #include <uhd/rfnoc/graph.hpp>
 #include <uhd/device3.hpp>
 
@@ -16,6 +17,9 @@ namespace uhd { namespace rfnoc {
 class graph_impl : public graph
 {
 public:
+    /************************************************************************
+     * Structors
+     ***********************************************************************/
     /*!
      * \param name An optional name to describe this graph
      * \param device_ptr Weak pointer to the originating device3
@@ -23,10 +27,10 @@ public:
      */
     graph_impl(
             const std::string &name,
-            boost::weak_ptr<uhd::device3> device_ptr
-            //async_msg_handler::sptr msg_handler
+            boost::weak_ptr<uhd::device3> device_ptr,
+            async_msg_handler::sptr msg_handler
     );
-    virtual ~graph_impl() {};
+    virtual ~graph_impl() {}
 
     /************************************************************************
      * Connection API
@@ -66,12 +70,25 @@ public:
 
 private:
 
+    void handle_overruns(const async_msg_t& async_msg);
+
+    //! Maps 16-bit addresses to block IDs
+    std::map<uint32_t, block_id_t> _block_id_map;
+
+    //! For any given block, look up the MIMO group
+    std::map<uint32_t, size_t> _mimo_group_map;
+
+    //! For any MIMO group, store the list of blocks in that group
+    std::map<size_t, std::set<block_id_t> > _mimo_groups;
+
     //! Optional: A string to describe this graph
     const std::string _name;
 
     //! Reference to the generating device object
     const boost::weak_ptr<uhd::device3> _device_ptr;
 
+    //! Reference to the async message handler
+    async_msg_handler::sptr _msg_handler;
 };
 
 }} /* namespace uhd::rfnoc */
diff --git a/host/lib/rfnoc/CMakeLists.txt b/host/lib/rfnoc/CMakeLists.txt
index f98518272..067963565 100644
--- a/host/lib/rfnoc/CMakeLists.txt
+++ b/host/lib/rfnoc/CMakeLists.txt
@@ -11,6 +11,7 @@
 
 LIBUHD_APPEND_SOURCES(
     # Infrastructure:
+    ${CMAKE_CURRENT_SOURCE_DIR}/async_msg_handler.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/block_ctrl_base.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/block_ctrl_base_factory.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/block_ctrl_impl.cpp
diff --git a/host/lib/rfnoc/async_msg_handler.cpp b/host/lib/rfnoc/async_msg_handler.cpp
new file mode 100644
index 000000000..b412eec9d
--- /dev/null
+++ b/host/lib/rfnoc/async_msg_handler.cpp
@@ -0,0 +1,190 @@
+//
+// Copyright 2018 Ettus Research, a National Instruments Company
+//
+// SPDX-License-Identifier: GPL-3.0-or-later
+//
+
+#include <uhd/exception.hpp>
+#include <uhd/utils/tasks.hpp>
+#include <uhd/utils/byteswap.hpp>
+#include <uhd/utils/log.hpp>
+#include <uhd/transport/chdr.hpp>
+#include <uhd/transport/zero_copy.hpp>
+#include <uhd/transport/bounded_buffer.hpp>
+#include <uhdlib/rfnoc/async_msg_handler.hpp>
+#include <boost/make_shared.hpp>
+#include <mutex>
+
+using namespace uhd;
+using namespace uhd::rfnoc;
+
+template <endianness_t _endianness>
+class async_msg_handler_impl : public async_msg_handler
+{
+public:
+    /************************************************************************
+     * Types
+     ***********************************************************************/
+    typedef uhd::transport::bounded_buffer<async_msg_t> async_md_type;
+
+    /************************************************************************
+     * Structors
+     ***********************************************************************/
+    async_msg_handler_impl(
+            uhd::transport::zero_copy_if::sptr recv,
+            uhd::transport::zero_copy_if::sptr send,
+            uhd::sid_t sid
+    ) : _rx_xport(recv),
+        _tx_xport(send),
+        _sid(sid)
+    {
+        // Launch receive thread
+        _recv_msg_task = task::make([=](){
+                this->handle_async_msgs();
+            }
+        );
+    }
+
+    ~async_msg_handler_impl() {}
+
+    /************************************************************************
+     * API calls
+     ***********************************************************************/
+    int register_event_handler(
+            const async_msg_t::event_code_t event_code,
+            async_handler_type handler
+    ) {
+        _event_handlers.insert(std::pair<async_msg_t::event_code_t, async_handler_type>(event_code, handler));
+        return _event_handlers.count(event_code);
+    }
+
+    void post_async_msg(
+            const async_msg_t &metadata
+    ) {
+        std::lock_guard<std::mutex> lock(_mutex);
+
+        for (auto const event_handler : _event_handlers) {
+            // If the event code in the message matches the event code used at
+            // registration time, call the event handler
+            if ((metadata.event_code & event_handler.first)
+                    == event_handler.first) {
+                event_handler.second(metadata);
+            }
+        }
+
+        // Print
+        if (metadata.event_code & async_msg_t::EVENT_CODE_UNDERFLOW) {
+            UHD_LOG_FASTPATH("U")
+        } else if (metadata.event_code &
+            ( async_msg_t::EVENT_CODE_SEQ_ERROR
+            | async_msg_t::EVENT_CODE_SEQ_ERROR_IN_BURST)
+        ) {
+            UHD_LOG_FASTPATH("S")
+        } else if (metadata.event_code &
+            ( async_msg_t::EVENT_CODE_LATE_CMD_ERROR
+            | async_msg_t::EVENT_CODE_LATE_DATA_ERROR)
+        ) {
+            UHD_LOG_FASTPATH("L")
+        } else if (metadata.event_code & async_msg_t::EVENT_CODE_OVERRUN) {
+            UHD_LOG_FASTPATH("O")
+        }
+    }
+
+private: // methods
+    /************************************************************************
+     * Internals
+     ***********************************************************************/
+    /*! Packet receiver thread call.
+     */
+    void handle_async_msgs(  )
+    {
+        using namespace uhd::transport;
+        managed_recv_buffer::sptr buff = _rx_xport->get_recv_buff();
+        if (not buff)
+            return;
+
+        // Get packet info
+        vrt::if_packet_info_t if_packet_info;
+        if_packet_info.num_packet_words32 = buff->size()/sizeof(uint32_t);
+        const uint32_t *packet_buff = buff->cast<const uint32_t *>();
+
+        //unpacking can fail
+        uint32_t (*endian_conv)(uint32_t) = uhd::ntohx;
+        try {
+            if (_endianness == ENDIANNESS_BIG) {
+                vrt::chdr::if_hdr_unpack_be(packet_buff, if_packet_info);
+                endian_conv = uhd::ntohx;
+            } else {
+                vrt::chdr::if_hdr_unpack_le(packet_buff, if_packet_info);
+                endian_conv = uhd::wtohx;
+            }
+        }
+        catch (const uhd::value_error &ex) {
+            UHD_LOGGER_ERROR("RFNOC") << "[async message handler] Error parsing async message packet: " << ex.what() << std::endl;
+            return;
+        }
+
+        // We discard anything that's not actually a command or response packet.
+        if (not (if_packet_info.packet_type & vrt::if_packet_info_t::PACKET_TYPE_CMD)
+                or if_packet_info.num_packet_words32 == 0) {
+            return;
+        }
+
+        const uint32_t *payload = packet_buff + if_packet_info.num_header_words32;
+        async_msg_t metadata(if_packet_info.num_payload_words32 - 1);
+        metadata.has_time_spec = if_packet_info.has_tsf;
+        // FIXME: not hardcoding tick rate
+        metadata.time_spec = time_spec_t::from_ticks(if_packet_info.tsf, 1);
+        metadata.event_code = async_msg_t::event_code_t(
+                endian_conv(payload[0]) & 0xFFFF
+        );
+        metadata.sid = if_packet_info.sid;
+
+        //load user payload
+        for (size_t i = 1; i < if_packet_info.num_payload_words32; i++) {
+            metadata.payload[i-1] = endian_conv(payload[i]);
+        }
+
+        this->post_async_msg(metadata);
+    }
+
+    uint32_t get_local_addr() const
+    {
+        return _sid.get_src();
+    }
+    
+private: // members
+
+    std::mutex _mutex;
+    //! Store event handlers
+    std::multimap<async_msg_t::event_code_t, async_handler_type> _event_handlers;
+    //! port that receive messge
+    uhd::transport::zero_copy_if::sptr _rx_xport;
+
+    //!port that send out respond
+    uhd::transport::zero_copy_if::sptr _tx_xport;
+
+    //! The source part of \p _sid is the address of this async message handler.
+    uhd::sid_t _sid;
+
+    //! Stores the task that polls the Rx queue
+    task::sptr _recv_msg_task;
+};
+
+async_msg_handler::sptr async_msg_handler::make(
+    uhd::transport::zero_copy_if::sptr recv,
+    uhd::transport::zero_copy_if::sptr send,
+    uhd::sid_t sid,
+    endianness_t endianness
+) {
+    if (endianness == ENDIANNESS_BIG) {
+        return boost::make_shared< async_msg_handler_impl<ENDIANNESS_BIG> >(
+            recv, send, sid
+        );
+    } else {
+        return boost::make_shared< async_msg_handler_impl<ENDIANNESS_LITTLE> >(
+            recv, send, sid
+        );
+    }
+}
+
diff --git a/host/lib/rfnoc/graph_impl.cpp b/host/lib/rfnoc/graph_impl.cpp
index d9e069993..0d90150c1 100644
--- a/host/lib/rfnoc/graph_impl.cpp
+++ b/host/lib/rfnoc/graph_impl.cpp
@@ -17,15 +17,15 @@ using namespace uhd::rfnoc;
  ***************************************************************************/
 graph_impl::graph_impl(
             const std::string &name,
-            boost::weak_ptr<uhd::device3> device_ptr
-            //async_msg_handler::sptr msg_handler
+            boost::weak_ptr<uhd::device3> device_ptr,
+            async_msg_handler::sptr msg_handler
 ) : _name(name)
   , _device_ptr(device_ptr)
+  , _msg_handler(msg_handler)
 {
     UHD_LOG_TRACE("RFNOC", "Instantiating RFNoC graph " << _name);
 }
 
-
 /****************************************************************************
  * Connection API
  ***************************************************************************/
@@ -154,6 +154,12 @@ void graph_impl::connect(
      * 5. Configure error policy
      ********************************************************************/
     dst->set_error_policy("next_burst");
+
+    /********************************************************************
+     * 6. Set async message handling
+     ********************************************************************/
+    src->sr_write(uhd::rfnoc::SR_RESP_OUT_DST_SID,  _msg_handler->get_local_addr(), src_block_port);
+    dst->sr_write(uhd::rfnoc::SR_RESP_IN_DST_SID, _msg_handler->get_local_addr(), dst_block_port);
 }
 
 void graph_impl::connect(
@@ -230,5 +236,5 @@ void graph_impl::connect_sink(
      * 5. Configure error policy
      ********************************************************************/
     dst->set_error_policy("next_burst");
-
 }
+
diff --git a/host/lib/usrp/device3/device3_impl.cpp b/host/lib/usrp/device3/device3_impl.cpp
index d8241ae74..fb74d4712 100644
--- a/host/lib/usrp/device3/device3_impl.cpp
+++ b/host/lib/usrp/device3/device3_impl.cpp
@@ -186,9 +186,36 @@ void device3_impl::enumerate_rfnoc_blocks(
 
 uhd::rfnoc::graph::sptr device3_impl::create_graph(const std::string &name)
 {
-    return boost::make_shared<uhd::rfnoc::graph_impl>(
+    // Create an async message handler
+    UHD_LOGGER_TRACE("DEVICE3") << "Creating async message handler for graph `" << name << "'...";
+    // FIXME: right now this only can only handle source sid of 0 and xbar local addr of 2.
+    // This is ok for now because that most of our device has xbard local addr hardcode to 2.
+    sid_t async_sid(0);
+    async_sid.set_dst_addr(2);
+    both_xports_t async_xports = make_transport(
+            async_sid,
+            ASYNC_MSG,
+            //FIXME: only get rx_hints from mb index of 0
+            get_rx_hints(0)
+    );
+    UHD_LOGGER_TRACE("DEVICE3") << " Async transport ready." << std::endl;
+    uhd::rfnoc::async_msg_handler::sptr async_msg_handler =
+        uhd::rfnoc::async_msg_handler::make(
+                async_xports.recv,
+                async_xports.send,
+                async_xports.send_sid,
+                async_xports.endianness
+        );
+    UHD_LOGGER_TRACE("DEVICE3") << "Async message has address " << async_xports.send_sid << std::endl;
+
+    // Create the graph
+    UHD_LOGGER_TRACE("DEVICE3") << "Creating graph `" << name << "'..." << std::endl;
+    uhd::rfnoc::graph::sptr graph = boost::make_shared<uhd::rfnoc::graph_impl>(
             name,
-            shared_from_this()
+            shared_from_this(),
+            async_msg_handler
     );
+
+    return graph;
 }