usrp1: implement soft time ctrl for send at, recv at

7 files changed, 275 insertions, 12 deletions

diff --git a/host/lib/usrp/usrp1/CMakeLists.txt b/host/lib/usrp/usrp1/CMakeLists.txt
index 519e17bfa..d72d67c0a 100644
--- a/host/lib/usrp/usrp1/CMakeLists.txt
+++ b/host/lib/usrp/usrp1/CMakeLists.txt
@@ -38,6 +38,7 @@ IF(ENABLE_USRP1)
         ${CMAKE_CURRENT_SOURCE_DIR}/dsp_impl.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/io_impl.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/mboard_impl.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/soft_time_ctrl.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/usrp1_iface.cpp
         ${CMAKE_CURRENT_SOURCE_DIR}/usrp1_iface.hpp
         ${CMAKE_CURRENT_SOURCE_DIR}/usrp1_impl.cpp
diff --git a/host/lib/usrp/usrp1/io_impl.cpp b/host/lib/usrp/usrp1/io_impl.cpp
index 7107294b6..a88b6503a 100644
--- a/host/lib/usrp/usrp1/io_impl.cpp
+++ b/host/lib/usrp/usrp1/io_impl.cpp
@@ -208,6 +208,7 @@ size_t usrp1_impl::send(
     const tx_metadata_t &metadata, const io_type_t &io_type,
     send_mode_t send_mode, double timeout
 ){
+    _soft_time_ctrl->send_pre(metadata, timeout);
     size_t num_samps_sent = vrt_packet_handler::send(
         _io_impl->packet_handler_send_state,       //last state of the send handler
         buffs, num_samps,                          //buffer to fill
@@ -294,6 +295,7 @@ size_t usrp1_impl::recv(
         0,                                         //vrt header offset
         _rx_subdev_spec.size()                     //num channels
     );
+    _soft_time_ctrl->recv_post(metadata, num_samps_recvd);
 
     //handle the polling for overflow conditions
     _io_impl->overflow_poll_samp_count += num_samps_recvd;
diff --git a/host/lib/usrp/usrp1/mboard_impl.cpp b/host/lib/usrp/usrp1/mboard_impl.cpp
index 4df5ada0a..6e10e86ce 100644
--- a/host/lib/usrp/usrp1/mboard_impl.cpp
+++ b/host/lib/usrp/usrp1/mboard_impl.cpp
@@ -240,16 +240,11 @@ void usrp1_impl::mboard_init(void)
     }
 }
 
-void usrp1_impl::issue_stream_cmd(const stream_cmd_t &stream_cmd)
-{
-    switch(stream_cmd.stream_mode){
-    case stream_cmd_t::STREAM_MODE_START_CONTINUOUS:
-        return _iface->write_firmware_cmd(VRQ_FPGA_SET_RX_ENABLE, true, 0, 0, 0);
-
-    case stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS:
-        return _iface->write_firmware_cmd(VRQ_FPGA_SET_RX_ENABLE, false, 0, 0, 0);
-
-    default: throw std::runtime_error("unsupported stream command type for USRP1");
+void usrp1_impl::stream_on_off(bool stream){
+    return _iface->write_firmware_cmd(VRQ_FPGA_SET_RX_ENABLE, stream, 0, 0, 0);
+    //drain any junk in the receive transport after stop streaming command
+    while(not stream and _data_transport->get_recv_buff(0.0).get() != NULL){
+        /* NOP */
     }
 }
 
@@ -326,6 +321,10 @@ void usrp1_impl::mboard_get(const wax::obj &key_, wax::obj &val)
         val = _iface->mb_eeprom;
         return;
 
+    case MBOARD_PROP_TIME_NOW:
+        val = _soft_time_ctrl->get_time();
+        return;
+
     default: UHD_THROW_PROP_GET_ERROR();
     }
 }
@@ -348,7 +347,7 @@ void usrp1_impl::mboard_set(const wax::obj &key, const wax::obj &val)
     switch(key.as<mboard_prop_t>()){
 
     case MBOARD_PROP_STREAM_CMD:
-        issue_stream_cmd(val.as<stream_cmd_t>());
+        _soft_time_ctrl->issue_stream_cmd(val.as<stream_cmd_t>());
         return;
 
     case MBOARD_PROP_RX_SUBDEV_SPEC:
@@ -384,6 +383,10 @@ void usrp1_impl::mboard_set(const wax::obj &key, const wax::obj &val)
         _iface->mb_eeprom = mboard_eeprom_t(*_iface, mboard_eeprom_t::MAP_B000);
         return;
 
+    case MBOARD_PROP_TIME_NOW:
+        _soft_time_ctrl->set_time(val.as<time_spec_t>());
+        return;
+
     default: UHD_THROW_PROP_SET_ERROR();
     }
 }
diff --git a/host/lib/usrp/usrp1/soft_time_ctrl.cpp b/host/lib/usrp/usrp1/soft_time_ctrl.cpp
new file mode 100644
index 000000000..8a6294690
--- /dev/null
+++ b/host/lib/usrp/usrp1/soft_time_ctrl.cpp
@@ -0,0 +1,179 @@
+//
+// Copyright 2011 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 "soft_time_ctrl.hpp"
+#include <uhd/transport/bounded_buffer.hpp>
+#include <boost/any.hpp>
+#include <boost/thread.hpp>
+#include <boost/date_time/posix_time/posix_time.hpp>
+#include <boost/date_time/local_time/local_time.hpp>
+#include <iostream>
+
+using namespace uhd;
+using namespace uhd::usrp;
+using namespace uhd::transport;
+namespace pt = boost::posix_time;
+namespace lt = boost::local_time;
+
+/***********************************************************************
+ * Utility helper functions
+ **********************************************************************/
+
+//TODO put these in time_spec_t (maybe useful)
+
+time_spec_t time_dur_to_time_spec(const pt::time_duration &time_dur){
+    return time_spec_t(
+        time_dur.total_seconds(),
+        time_dur.fractional_seconds(),
+        pt::time_duration::ticks_per_second()
+    );
+}
+
+pt::time_duration time_spec_to_time_dur(const time_spec_t &time_spec){
+    return pt::time_duration(
+        0, 0, time_spec.get_full_secs(),
+        time_spec.get_tick_count(pt::time_duration::ticks_per_second())
+    );
+}
+
+/***********************************************************************
+ * Soft time control implementation
+ **********************************************************************/
+class soft_time_ctrl_impl : public soft_time_ctrl{
+public:
+    soft_time_ctrl_impl(const cb_fcn_type &stream_on_off):
+        _nsamps_remaining(0),
+        _stream_mode(stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS),
+        _cmd_queue(bounded_buffer<boost::any>::make(2)),
+        _stream_on_off(stream_on_off)
+    {
+        //synchronously spawn a new thread
+        _update_mutex.lock(); //lock mutex before spawned
+        _thread_group.create_thread(boost::bind(&soft_time_ctrl_impl::recv_cmd_dispatcher, this));
+        _update_mutex.lock(); //lock blocks until spawned
+        _update_mutex.unlock(); //unlock mutex before done
+    }
+
+    ~soft_time_ctrl_impl(void){
+        _thread_running = false;
+        _thread_group.join_all();
+    }
+
+    void set_time(const time_spec_t &time){
+        _time_offset = pt::microsec_clock::universal_time() - time_spec_to_time_dur(time);
+    }
+
+    time_spec_t get_time(void){
+        return time_dur_to_time_spec(pt::microsec_clock::universal_time() - _time_offset);
+    }
+
+    void recv_post(rx_metadata_t &md, size_t &nsamps){
+        //load the metadata with the current time
+        md.has_time_spec = true;
+        md.time_spec = get_time();
+
+        //lock the mutex here before changing state
+        boost::mutex::scoped_lock lock(_update_mutex);
+
+        //When to stop streaming:
+        //The samples have been received and the stream mode is non-continuous.
+        //Rewrite the sample count to clip to the requested number of samples.
+        if (_nsamps_remaining <= nsamps and
+            _stream_mode != stream_cmd_t::STREAM_MODE_START_CONTINUOUS
+        ){
+            nsamps = _nsamps_remaining; //set nsamps, then stop
+            stream_on_off(false);
+            return;
+        }
+
+        //update the consumed samples
+        _nsamps_remaining -= nsamps;
+    }
+
+    void send_pre(const tx_metadata_t &md, double /*TODO timeout*/){
+        if (not md.has_time_spec) return;
+        sleep_until_time(md.time_spec); //TODO late?
+    }
+
+    void issue_stream_cmd(const stream_cmd_t &cmd){
+        _cmd_queue->push_with_wait(cmd);
+    }
+
+private:
+
+    void sleep_until_time(const time_spec_t &time){
+        boost::this_thread::sleep(_time_offset + time_spec_to_time_dur(time));
+    }
+
+    void recv_cmd_handle_cmd(const stream_cmd_t &cmd){
+        //handle the stream at time by sleeping
+        if (not cmd.stream_now) sleep_until_time(cmd.time_spec); //TODO late?
+
+        //lock the mutex here before changing state
+        boost::mutex::scoped_lock lock(_update_mutex);
+
+        //When to stop streaming:
+        //Stop streaming when the command is a stop and streaming.
+        if (cmd.stream_mode == stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS
+           and _stream_mode != stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS
+        ) stream_on_off(false);
+
+        //When to start streaming:
+        //Start streaming when the command is not a stop and not streaming.
+        if (cmd.stream_mode != stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS
+           and _stream_mode == stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS
+        ) stream_on_off(true);
+
+        //update the state
+        _nsamps_remaining += cmd.num_samps;
+        _stream_mode = cmd.stream_mode;
+    }
+
+    void recv_cmd_dispatcher(void){
+        _thread_running = true;
+        _update_mutex.unlock();
+
+        boost::any cmd;
+        while (_thread_running){
+            if (_cmd_queue->pop_with_timed_wait(cmd, 1.0)){
+                recv_cmd_handle_cmd(boost::any_cast<stream_cmd_t>(cmd));
+            }
+        }
+    }
+
+    void stream_on_off(bool stream){
+        _stream_on_off(stream);
+        _nsamps_remaining = 0;
+    }
+
+    boost::mutex _update_mutex;
+    size_t _nsamps_remaining;
+    stream_cmd_t::stream_mode_t _stream_mode;
+
+    pt::ptime _time_offset;
+    bounded_buffer<boost::any>::sptr _cmd_queue;
+    const cb_fcn_type _stream_on_off;
+    boost::thread_group _thread_group;
+    bool _thread_running;
+};
+
+/***********************************************************************
+ * Soft time control factor
+ **********************************************************************/
+soft_time_ctrl::sptr soft_time_ctrl::make(const cb_fcn_type &stream_on_off){
+    return sptr(new soft_time_ctrl_impl(stream_on_off));
+}
diff --git a/host/lib/usrp/usrp1/soft_time_ctrl.hpp b/host/lib/usrp/usrp1/soft_time_ctrl.hpp
new file mode 100644
index 000000000..42056c285
--- /dev/null
+++ b/host/lib/usrp/usrp1/soft_time_ctrl.hpp
@@ -0,0 +1,70 @@
+//
+// Copyright 2011 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/>.
+//
+
+#ifndef INCLUDED_LIBUHD_USRP_USRP1_SOFT_TIME_CTRL_HPP
+#define INCLUDED_LIBUHD_USRP_USRP1_SOFT_TIME_CTRL_HPP
+
+#include <uhd/types/stream_cmd.hpp>
+#include <uhd/types/time_spec.hpp>
+#include <uhd/types/metadata.hpp>
+#include <boost/utility.hpp>
+#include <boost/shared_ptr.hpp>
+#include <boost/function.hpp>
+
+namespace uhd{ namespace usrp{
+
+/*!
+ * The soft time control emulates some of the
+ * advanced streaming capabilities of the later USRP models.
+ * Soft time control uses the system time to emulate
+ * timed transmits, timed receive commands, device time,
+ * and inline and async error messages.
+ */
+class soft_time_ctrl : boost::noncopyable{
+public:
+    typedef boost::shared_ptr<soft_time_ctrl> sptr;
+    typedef boost::function<void(bool)> cb_fcn_type;
+
+    /*!
+     * Make a new soft time control.
+     * \param start_streaming a function callback to start streaming
+     * \param stop_streaming a function callback to stop streaming
+     * \return a new soft time control object
+     */
+    static sptr make(const cb_fcn_type &stream_on_off);
+        //TODO pass in the error queue for async msgs
+        //TODO pass in the queue for inline msgs
+
+    //! Set the current time
+    virtual void set_time(const time_spec_t &time) = 0;
+
+    //! Get the current time
+    virtual time_spec_t get_time(void) = 0;
+
+    //! Call after the internal recv function
+    virtual void recv_post(rx_metadata_t &md, size_t &nsamps) = 0;
+
+    //! Call before the internal send function
+    virtual void send_pre(const tx_metadata_t &md, double timeout) = 0;
+
+    //! Issue a stream command to receive
+    virtual void issue_stream_cmd(const stream_cmd_t &cmd) = 0;
+};
+
+}} //namespace
+
+#endif /* INCLUDED_LIBUHD_USRP_USRP1_SOFT_TIME_CTRL_HPP */
diff --git a/host/lib/usrp/usrp1/usrp1_impl.cpp b/host/lib/usrp/usrp1/usrp1_impl.cpp
index 6016b0979..21f2b148a 100644
--- a/host/lib/usrp/usrp1/usrp1_impl.cpp
+++ b/host/lib/usrp/usrp1/usrp1_impl.cpp
@@ -161,6 +161,10 @@ usrp1_impl::usrp1_impl(uhd::transport::usb_zero_copy::sptr data_transport,
                        usrp_ctrl::sptr ctrl_transport)
  : _data_transport(data_transport), _ctrl_transport(ctrl_transport)
 {
+    _soft_time_ctrl = soft_time_ctrl::make(
+        boost::bind(&usrp1_impl::stream_on_off, this, _1)
+    );
+
     _iface = usrp1_iface::make(ctrl_transport);
 
     //create clock interface
diff --git a/host/lib/usrp/usrp1/usrp1_impl.hpp b/host/lib/usrp/usrp1/usrp1_impl.hpp
index ff4d40762..d92ce6e79 100644
--- a/host/lib/usrp/usrp1/usrp1_impl.hpp
+++ b/host/lib/usrp/usrp1/usrp1_impl.hpp
@@ -19,6 +19,7 @@
 #include "usrp1_ctrl.hpp"
 #include "clock_ctrl.hpp"
 #include "codec_ctrl.hpp"
+#include "soft_time_ctrl.hpp"
 #include <uhd/device.hpp>
 #include <uhd/utils/pimpl.hpp>
 #include <uhd/types/dict.hpp>
@@ -114,13 +115,16 @@ private:
         const uhd::usrp::dboard_id_t &rx_dboard_id
     );
 
+    //soft time control emulation
+    uhd::usrp::soft_time_ctrl::sptr _soft_time_ctrl;
+
     //interface to ioctls and file descriptor
     usrp1_iface::sptr _iface;
 
     //handle io stuff
     UHD_PIMPL_DECL(io_impl) _io_impl;
     void io_init(void);
-    void issue_stream_cmd(const uhd::stream_cmd_t &stream_cmd);
+    void stream_on_off(bool);
     void handle_overrun(size_t);
 
     //underrun and overrun poll intervals