usrp-e: untested attempt at zero copy iface for mmap

3 files changed, 212 insertions, 87 deletions

diff --git a/host/lib/usrp/usrp_e/CMakeLists.txt b/host/lib/usrp/usrp_e/CMakeLists.txt
index da759d931..bab868f90 100644
--- a/host/lib/usrp/usrp_e/CMakeLists.txt
+++ b/host/lib/usrp/usrp_e/CMakeLists.txt
@@ -54,6 +54,7 @@ IF(ENABLE_USRP_E)
         ${CMAKE_SOURCE_DIR}/lib/usrp/usrp_e/usrp_e_impl.hpp
         ${CMAKE_SOURCE_DIR}/lib/usrp/usrp_e/usrp_e_iface.cpp
         ${CMAKE_SOURCE_DIR}/lib/usrp/usrp_e/usrp_e_iface.hpp
+        ${CMAKE_SOURCE_DIR}/lib/usrp/usrp_e/usrp_e_mmap_zero_copy.cpp
         ${CMAKE_SOURCE_DIR}/lib/usrp/usrp_e/usrp_e_regs.hpp
     )
 ELSE(ENABLE_USRP_E)
diff --git a/host/lib/usrp/usrp_e/io_impl.cpp b/host/lib/usrp/usrp_e/io_impl.cpp
index e57d6ce35..0b5fa054b 100644
--- a/host/lib/usrp/usrp_e/io_impl.cpp
+++ b/host/lib/usrp/usrp_e/io_impl.cpp
@@ -22,8 +22,6 @@
 #include <uhd/transport/bounded_buffer.hpp>
 #include "../../transport/vrt_packet_handler.hpp"
 #include <boost/bind.hpp>
-#include <fcntl.h> //read, write
-#include <poll.h>
 #include <boost/format.hpp>
 #include <boost/thread.hpp>
 #include <iostream>
@@ -32,91 +30,16 @@ using namespace uhd;
 using namespace uhd::usrp;
 using namespace uhd::transport;
 
+zero_copy_if::sptr usrp_e_make_mmap_zero_copy(usrp_e_iface::sptr iface);
+
 /***********************************************************************
  * Constants
  **********************************************************************/
-static const size_t MAX_BUFF_SIZE = 2048;
-static const bool usrp_e_io_impl_verbose = false;
 static const size_t tx_async_report_sid = 1;
 static const int underflow_flags = async_metadata_t::EVENT_CODE_UNDERFLOW | async_metadata_t::EVENT_CODE_UNDERFLOW_IN_PACKET;
 static const double recv_timeout_ms = 100;
 
 /***********************************************************************
- * Data Transport (phony zero-copy with read/write)
- **********************************************************************/
-class data_transport:
-    public transport::phony_zero_copy_recv_if,
-    public transport::phony_zero_copy_send_if
-{
-public:
-    data_transport(int fd):
-        transport::phony_zero_copy_recv_if(MAX_BUFF_SIZE),
-        transport::phony_zero_copy_send_if(MAX_BUFF_SIZE),
-        _fd(fd)
-    {
-        /* NOP */
-    }
-
-    size_t get_num_recv_frames(void) const{
-        return 100; //FIXME no idea!
-        //this will be an important number when packet ring gets implemented
-    }
-
-    size_t get_num_send_frames(void) const{
-        return 100; //FIXME no idea!
-        //this will be an important number when packet ring gets implemented
-    }
-
-private:
-    int _fd;
-    ssize_t send(const boost::asio::const_buffer &buff){
-        return write(_fd,
-            boost::asio::buffer_cast<const void *>(buff),
-            boost::asio::buffer_size(buff)
-        );
-    }
-    ssize_t recv(const boost::asio::mutable_buffer &buff, size_t to_ms){
-        //std::cout << boost::format(
-        //    "calling read on fd %d, buff size is %d"
-        //) % _fd % boost::asio::buffer_size(buff) << std::endl;
-
-        //setup and call poll on the file descriptor
-        //return 0 and do not read when poll times out
-        pollfd pfd;
-        pfd.fd = _fd;
-        pfd.events = POLLIN;
-        ssize_t poll_ret = poll(&pfd, 1, to_ms);
-        if (poll_ret <= 0){
-            if (usrp_e_io_impl_verbose) std::cerr << boost::format(
-                "usrp-e io impl recv(): poll() returned non-positive value: %d\n"
-                "    -> return 0 for timeout"
-            ) % poll_ret << std::endl;
-            return 0; //timeout
-        }
-
-        //perform the blocking read(...)
-        ssize_t read_ret = read(_fd,
-            boost::asio::buffer_cast<void *>(buff),
-            boost::asio::buffer_size(buff)
-        );
-        if (read_ret < 0){
-            if (usrp_e_io_impl_verbose) std::cerr << boost::format(
-                "usrp-e io impl recv(): read() returned small value: %d\n"
-                "    -> return -1 for error"
-            ) % read_ret << std::endl;
-            return -1;
-        }
-
-        //std::cout << "len " << int(read_ret) << std::endl;
-        //for (size_t i = 0; i < 9; i++){
-        //    std::cout << boost::format("    0x%08x") % boost::asio::buffer_cast<boost::uint32_t *>(buff)[i] << std::endl;
-        //}
-
-        return read_ret;
-    }
-};
-
-/***********************************************************************
  * io impl details (internal to this file)
  * - pirate crew of 1
  * - bounded buffer
@@ -127,11 +50,11 @@ struct usrp_e_impl::io_impl{
     //state management for the vrt packet handler code
     vrt_packet_handler::recv_state packet_handler_recv_state;
     vrt_packet_handler::send_state packet_handler_send_state;
-    data_transport transport;
+    zero_copy_if::sptr data_xport;
     bool continuous_streaming;
-    io_impl(int fd):
-        transport(fd),
-        recv_pirate_booty(recv_booty_type::make(transport.get_num_recv_frames())),
+    io_impl(usrp_e_iface::sptr iface):
+        data_xport(usrp_e_make_mmap_zero_copy(iface)),
+        recv_pirate_booty(recv_booty_type::make(data_xport->get_num_recv_frames())),
         async_msg_fifo(bounded_buffer<async_metadata_t>::make(100/*messages deep*/))
     {
         /* NOP */
@@ -171,7 +94,7 @@ void usrp_e_impl::io_impl::recv_pirate_loop(
     //size_t next_packet_seq = 0;
 
     while(recv_pirate_crew_raiding){
-        managed_recv_buffer::sptr buff = this->transport.get_recv_buff(recv_timeout_ms);
+        managed_recv_buffer::sptr buff = this->data_xport->get_recv_buff(recv_timeout_ms);
         if (not buff.get()) continue; //ignore timeout/error buffers
 
         try{
@@ -229,7 +152,7 @@ void usrp_e_impl::io_init(void){
     _iface->poke32(UE_REG_CTRL_TX_REPORT_SID, tx_async_report_sid);
     _iface->poke32(UE_REG_CTRL_TX_POLICY, UE_FLAG_CTRL_TX_POLICY_NEXT_PACKET);
 
-    _io_impl = UHD_PIMPL_MAKE(io_impl, (_iface->get_file_descriptor()));
+    _io_impl = UHD_PIMPL_MAKE(io_impl, (_iface));
 
     //spawn a pirate, yarrr!
     _io_impl->recv_pirate_crew.create_thread(boost::bind(
@@ -258,7 +181,7 @@ void usrp_e_impl::handle_overrun(size_t){
  * Data Send
  **********************************************************************/
 bool get_send_buffs(
-    data_transport *trans,
+    zero_copy_if::sptr trans,
     vrt_packet_handler::managed_send_buffs_t &buffs
 ){
     UHD_ASSERT_THROW(buffs.size() == 1);
@@ -285,7 +208,7 @@ size_t usrp_e_impl::send(
         io_type, send_otw_type,                    //input and output types to convert
         MASTER_CLOCK_RATE,                         //master clock tick rate
         uhd::transport::vrt::if_hdr_pack_le,
-        boost::bind(&get_send_buffs, &_io_impl->transport, _1),
+        boost::bind(&get_send_buffs, _io_impl->data_xport, _1),
         get_max_send_samps_per_packet()
     );
 }
diff --git a/host/lib/usrp/usrp_e/usrp_e_mmap_zero_copy.cpp b/host/lib/usrp/usrp_e/usrp_e_mmap_zero_copy.cpp
new file mode 100644
index 000000000..0910caa6f
--- /dev/null
+++ b/host/lib/usrp/usrp_e/usrp_e_mmap_zero_copy.cpp
@@ -0,0 +1,201 @@
+//
+// Copyright 2010 Ettus Research LLC
+//
+// 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/>.
+//
+
+#include <uhd/transport/zero_copy.hpp>
+#include <uhd/utils/assert.hpp>
+#include <linux/usrp_e.h>
+#include <sys/mman.h> //mmap
+#include <unistd.h> //getpagesize
+#include <poll.h> //poll
+#include <boost/cstdint.hpp>
+#include "usrp_e_iface.hpp"
+
+using namespace uhd;
+using namespace uhd::transport;
+
+static const bool debug_verbose = false;
+
+/***********************************************************************
+ * The managed receive buffer implementation
+ **********************************************************************/
+class usrp_e_mmap_managed_recv_buffer : public managed_recv_buffer{
+public:
+    usrp_e_mmap_managed_recv_buffer(
+        const void *mem, size_t len, ring_buffer_info *info
+    ):
+        _buff(mem, len), _info(info)
+    {
+        /* NOP */
+    }
+
+    ~usrp_e_mmap_managed_recv_buffer(void){
+        _info->flags = RB_KERNEL;
+    }
+
+private:
+    const boost::asio::const_buffer &get(void) const{
+        return _buff;
+    }
+
+    const boost::asio::const_buffer _buff;
+    ring_buffer_info *_info;
+};
+
+/***********************************************************************
+ * The managed send buffer implementation
+ **********************************************************************/
+class usrp_e_mmap_managed_send_buffer : public managed_send_buffer{
+public:
+    usrp_e_mmap_managed_send_buffer(
+        void *mem, size_t len, ring_buffer_info *info, int fd
+    ):
+        _buff(mem, len), _info(info), _fd(fd), _commited(false)
+    {
+        /* NOP */
+    }
+
+    ~usrp_e_mmap_managed_send_buffer(void){
+        if (not _commited) this->commit(0);
+    }
+
+    ssize_t commit(size_t num_bytes){
+        _commited = true;
+        _info->len = num_bytes;
+        _info->flags = RB_USER;
+        ssize_t ret = ::write(_fd, NULL, 0);
+        return (ret < 0)? ret : num_bytes;
+    }
+
+private:
+    const boost::asio::mutable_buffer &get(void) const{
+        return _buff;
+    }
+
+    const boost::asio::mutable_buffer _buff;
+    ring_buffer_info *_info;
+    int _fd;
+    bool _commited;
+};
+
+/***********************************************************************
+ * The zero copy interface implementation
+ **********************************************************************/
+class usrp_e_mmap_zero_copy_impl : public zero_copy_if{
+public:
+    usrp_e_mmap_zero_copy_impl(usrp_e_iface::sptr iface):
+        _fd(iface->get_file_descriptor()), _recv_index(0), _send_index(0)
+    {
+        //get system sizes
+        iface->ioctl(USRP_E_GET_RB_INFO, &_rb_size);
+        size_t page_size = getpagesize();
+        _frame_size = page_size/2;
+
+        //calculate the memory size
+        size_t map_size =
+            (_rb_size.num_pages_rx_flags + _rb_size.num_pages_tx_flags) * page_size +
+            (_rb_size.num_rx_frames + _rb_size.num_tx_frames) * _frame_size;
+
+        //call mmap to get the memory
+        void *ring_buffer = mmap(
+            NULL, map_size, PROT_READ | PROT_WRITE, MAP_SHARED, _fd, 0
+        );
+        UHD_ASSERT_THROW(ring_buffer != MAP_FAILED);
+
+        //calculate the memory offsets for info and buffers
+        size_t recv_info_off = 0;
+        size_t recv_buff_off = recv_info_off + (_rb_size.num_pages_rx_flags * page_size);
+        size_t send_info_off = recv_buff_off + (_rb_size.num_rx_frames * _frame_size);
+        size_t send_buff_off = send_info_off + (_rb_size.num_pages_tx_flags * page_size);
+
+        //set the internal pointers for info and buffers
+        typedef ring_buffer_info (*rbi_pta)[];
+        boost::uint8_t *rb_ptr = reinterpret_cast<boost::uint8_t *>(ring_buffer);
+        _recv_info = reinterpret_cast<rbi_pta>(rb_ptr + recv_info_off);
+        _recv_buff = rb_ptr + recv_buff_off;
+        _send_info = reinterpret_cast<rbi_pta>(rb_ptr + send_info_off);
+        _send_buff = rb_ptr + send_buff_off;
+    }
+
+    managed_recv_buffer::sptr get_recv_buff(size_t timeout_ms){
+        //grab pointers to the info and buffer
+        ring_buffer_info *info = (*_recv_info) + _recv_index;
+        void *mem = _recv_buff + _frame_size*_recv_index;
+
+        //poll/wait for a ready frame
+        if (not (info->flags & RB_USER)){
+            pollfd pfd;
+            pfd.fd = _fd;
+            pfd.events = POLLIN;
+            ssize_t poll_ret = poll(&pfd, 1, timeout_ms);
+            if (poll_ret <= 0) return managed_recv_buffer::sptr();
+        }
+
+        //increment the index for the next call
+        if (++_recv_index == size_t(_rb_size.num_rx_frames)) _recv_index = 0;
+
+        //return the managed buffer for this frame
+        return managed_recv_buffer::sptr(
+            new usrp_e_mmap_managed_recv_buffer(mem, info->len, info)
+        );
+    }
+
+    size_t get_num_recv_frames(void) const{
+        return _rb_size.num_rx_frames;
+    }
+
+    managed_send_buffer::sptr get_send_buff(void){
+        //grab pointers to the info and buffer
+        ring_buffer_info *info = (*_send_info) + _send_index;
+        void *mem = _send_buff + _frame_size*_send_index;
+
+        //poll/wait for a ready frame
+        if (not (info->flags & RB_KERNEL)){
+            pollfd pfd;
+            pfd.fd = _fd;
+            pfd.events = POLLOUT;
+            ssize_t poll_ret = poll(&pfd, 1, -1 /*forever*/);
+            if (poll_ret <= 0) return managed_send_buffer::sptr();
+        }
+
+        //increment the index for the next call
+        if (++_send_index == size_t(_rb_size.num_tx_frames)) _send_index = 0;
+
+        //return the managed buffer for this frame
+        return managed_send_buffer::sptr(
+            new usrp_e_mmap_managed_send_buffer(mem, _frame_size, info, _fd)
+        );
+    }
+
+    size_t get_num_send_frames(void) const{
+        return _rb_size.num_tx_frames;
+    }
+
+private:
+    int _fd;
+    usrp_e_ring_buffer_size_t _rb_size;
+    size_t _frame_size;
+    ring_buffer_info (*_recv_info)[], (*_send_info)[];
+    boost::uint8_t *_recv_buff, *_send_buff;
+    size_t _recv_index, _send_index;
+};
+
+/***********************************************************************
+ * The zero copy interface make function
+ **********************************************************************/
+zero_copy_if::sptr usrp_e_make_mmap_zero_copy(usrp_e_iface::sptr iface){
+    return zero_copy_if::sptr(new usrp_e_mmap_zero_copy_impl(iface));
+}