implemented dsp and rx control

4 files changed, 182 insertions, 13 deletions

diff --git a/host/lib/usrp/usrp_e/dsp_impl.cpp b/host/lib/usrp/usrp_e/dsp_impl.cpp
index e61f529ab..11d22d15f 100644
--- a/host/lib/usrp/usrp_e/dsp_impl.cpp
+++ b/host/lib/usrp/usrp_e/dsp_impl.cpp
@@ -15,14 +15,34 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 
-#include <boost/bind.hpp>
 #include "usrp_e_impl.hpp"
+#include "usrp_e_regs.hpp"
+#include <uhd/usrp/dsp_props.hpp>
+#include <uhd/utils/assert.hpp>
+#include <boost/bind.hpp>
+#include <boost/math/special_functions/round.hpp>
+
+#define rint boost::math::iround
 
+using namespace uhd;
 using namespace uhd::usrp;
 
 /***********************************************************************
  * Helper Functions
  **********************************************************************/
+static boost::uint32_t calculate_freq_word_and_update_actual_freq(double &freq, double clock_freq){
+    UHD_ASSERT_THROW(std::abs(freq) < clock_freq/2.0);
+    static const double scale_factor = std::pow(2.0, 32);
+
+    //calculate the freq register word
+    boost::uint32_t freq_word = rint((freq / clock_freq) * scale_factor);
+
+    //update the actual frequency
+    freq = (double(freq_word) / scale_factor) * clock_freq;
+
+    return freq_word;
+}
+
 // Check if requested decim/interp rate is:
 //      multiple of 4, enable two halfband filters
 //      multiple of 2, enable one halfband filter
@@ -57,15 +77,62 @@ void usrp_e_impl::rx_ddc_init(void){
 /***********************************************************************
  * RX DDC Get
  **********************************************************************/
-void usrp_e_impl::rx_ddc_get(const wax::obj &, wax::obj &){
-    UHD_THROW_PROP_GET_ERROR();
+void usrp_e_impl::rx_ddc_get(const wax::obj &key, wax::obj &val){
+    switch(key.as<dsp_prop_t>()){
+    case DSP_PROP_NAME:
+        val = std::string("usrp-e ddc0");
+        return;
+
+    case DSP_PROP_OTHERS:
+        val = prop_names_t(); //empty
+        return;
+
+    case DSP_PROP_FREQ_SHIFT:
+        val = _ddc_freq;
+        return;
+
+    case DSP_PROP_CODEC_RATE:
+        val = MASTER_CLOCK_RATE;
+        return;
+
+    case DSP_PROP_HOST_RATE:
+        val = MASTER_CLOCK_RATE/_ddc_decim;
+        return;
+
+    default: UHD_THROW_PROP_GET_ERROR();
+    }
 }
 
 /***********************************************************************
  * RX DDC Set
  **********************************************************************/
-void usrp_e_impl::rx_ddc_set(const wax::obj &, const wax::obj &){
-    UHD_THROW_PROP_SET_ERROR();
+void usrp_e_impl::rx_ddc_set(const wax::obj &key, const wax::obj &val){
+    switch(key.as<dsp_prop_t>()){
+
+    case DSP_PROP_FREQ_SHIFT:{
+            double new_freq = val.as<double>();
+            _iface->poke32(UE_REG_DSP_RX_FREQ,
+                calculate_freq_word_and_update_actual_freq(new_freq, MASTER_CLOCK_RATE)
+            );
+            _ddc_freq = new_freq; //shadow
+        }
+        return;
+
+    case DSP_PROP_HOST_RATE:{
+            //set the decimation
+            _ddc_decim = rint(MASTER_CLOCK_RATE/val.as<double>());
+            _iface->poke32(UE_REG_DSP_RX_DECIM_RATE, calculate_cic_word(_ddc_decim));
+
+            //set the scaling
+            static const boost::int16_t default_rx_scale_iq = 1024;
+            _iface->poke32(UE_REG_DSP_RX_SCALE_IQ,
+                calculate_iq_scale_word(default_rx_scale_iq, default_rx_scale_iq)
+            );
+        }
+        return;
+
+    default: UHD_THROW_PROP_SET_ERROR();
+    }
 }
 
 /***********************************************************************
@@ -81,13 +148,65 @@ void usrp_e_impl::tx_duc_init(void){
 /***********************************************************************
  * TX DUC Get
  **********************************************************************/
-void usrp_e_impl::tx_duc_get(const wax::obj &, wax::obj &){
-    UHD_THROW_PROP_GET_ERROR();
+void usrp_e_impl::tx_duc_get(const wax::obj &key, wax::obj &val){
+    switch(key.as<dsp_prop_t>()){
+    case DSP_PROP_NAME:
+        val = std::string("usrp-e duc0");
+        return;
+
+    case DSP_PROP_OTHERS:
+        val = prop_names_t(); //empty
+        return;
+
+    case DSP_PROP_FREQ_SHIFT:
+        val = _duc_freq;
+        return;
+
+    case DSP_PROP_CODEC_RATE:
+        val = MASTER_CLOCK_RATE;
+        return;
+
+    case DSP_PROP_HOST_RATE:
+        val = MASTER_CLOCK_RATE/_duc_interp;
+        return;
+
+    default: UHD_THROW_PROP_GET_ERROR();
+    }
 }
 
 /***********************************************************************
  * TX DUC Set
  **********************************************************************/
-void usrp_e_impl::tx_duc_set(const wax::obj &, const wax::obj &){
-    UHD_THROW_PROP_SET_ERROR();
+void usrp_e_impl::tx_duc_set(const wax::obj &key, const wax::obj &val){
+    switch(key.as<dsp_prop_t>()){
+
+    case DSP_PROP_FREQ_SHIFT:{
+            double new_freq = val.as<double>();
+            _iface->poke32(UE_REG_DSP_TX_FREQ,
+                calculate_freq_word_and_update_actual_freq(new_freq, MASTER_CLOCK_RATE)
+            );
+            _duc_freq = new_freq; //shadow
+        }
+        return;
+
+    case DSP_PROP_HOST_RATE:{
+            _duc_interp = rint(MASTER_CLOCK_RATE/val.as<double>());
+
+            // Calculate CIC interpolation (i.e., without halfband interpolators)
+            size_t tmp_interp = calculate_cic_word(_duc_interp) & 0xff;
+
+            // Calculate closest multiplier constant to reverse gain absent scale multipliers
+            double interp_cubed = std::pow(double(tmp_interp), 3);
+            boost::int16_t scale = rint((4096*std::pow(2, ceil(log2(interp_cubed))))/(1.65*interp_cubed));
+
+            //set the interpolation
+            _iface->poke32(UE_REG_DSP_TX_INTERP_RATE, calculate_cic_word(_duc_interp));
+
+            //set the scaling
+            _iface->poke32(UE_REG_DSP_TX_SCALE_IQ, calculate_iq_scale_word(scale, scale));
+        }
+        return;
+
+    default: UHD_THROW_PROP_SET_ERROR();
+    }
 }
diff --git a/host/lib/usrp/usrp_e/io_impl.cpp b/host/lib/usrp/usrp_e/io_impl.cpp
index f83829cb8..5914bc613 100644
--- a/host/lib/usrp/usrp_e/io_impl.cpp
+++ b/host/lib/usrp/usrp_e/io_impl.cpp
@@ -16,6 +16,7 @@
 //
 
 #include "usrp_e_impl.hpp"
+#include "usrp_e_regs.hpp"
 #include "../../transport/vrt_packet_handler.hpp"
 #include <boost/bind.hpp>
 #include <fcntl.h> //read, write
@@ -86,9 +87,44 @@ struct usrp_e_impl::io_impl{
 };
 
 void usrp_e_impl::io_init(void){
+    //setup rx data path
+    _iface->poke32(UE_REG_CTRL_RX_NSAMPS_PER_PKT, 1000); //FIXME magic number
+    _iface->poke32(UE_REG_CTRL_RX_NCHANNELS, 1);
+    _iface->poke32(UE_REG_CTRL_RX_CLEAR_OVERRUN, 1); //reset
+    _iface->poke32(UE_REG_CTRL_RX_VRT_HEADER, 0
+        | (0x1 << 28) //if data with stream id
+        | (0x1 << 26) //has trailer
+        | (0x3 << 22) //integer time other
+        | (0x1 << 20) //fractional time sample count
+    );
+    _iface->poke32(UE_REG_CTRL_RX_VRT_STREAM_ID, 0);
+    _iface->poke32(UE_REG_CTRL_RX_VRT_TRAILER, 0);
+
     _io_impl = UHD_PIMPL_MAKE(io_impl, (_iface->get_file_descriptor()));
 }
 
+static boost::uint32_t make_stream_cmd(bool now, bool chain, boost::uint32_t nsamps){
+    return (((now)? 1 : 0) << 31) | (((chain)? 1 : 0) << 30) | nsamps;
+}
+
+void usrp_e_impl::issue_stream_cmd(const stream_cmd_t &stream_cmd){
+    boost::uint32_t cmd = 0;
+    switch(stream_cmd.stream_mode){
+    case stream_cmd_t::STREAM_MODE_NUM_SAMPS_AND_DONE:
+        cmd = make_stream_cmd(stream_cmd.stream_now, false, stream_cmd.num_samps);
+        break;
+
+    case stream_cmd_t::STREAM_MODE_NUM_SAMPS_AND_MORE:
+        cmd = make_stream_cmd(stream_cmd.stream_now, true, stream_cmd.num_samps);
+        break;
+
+    default: throw std::runtime_error("stream mode not implemented");
+    }
+    _iface->poke32(UE_REG_CTRL_RX_STREAM_CMD, cmd);
+    _iface->poke32(UE_REG_CTRL_RX_TIME_SECS,  stream_cmd.time_spec.secs);
+    _iface->poke32(UE_REG_CTRL_RX_TIME_TICKS, stream_cmd.time_spec.get_ticks(MASTER_CLOCK_RATE));
+}
+
 /***********************************************************************
  * Data Send
  **********************************************************************/
@@ -110,7 +146,7 @@ size_t usrp_e_impl::send(
         send_mode,
         io_type,
         send_otw_type, //TODO
-        64e6, //TODO
+        MASTER_CLOCK_RATE,
         boost::bind(&data_transport::get_send_buff, &_io_impl->transport),
         (MAX_BUFF_SIZE - sizeof(usrp_transfer_frame))/send_otw_type.get_sample_size(),
         offsetof(usrp_transfer_frame, buf)
@@ -138,7 +174,7 @@ size_t usrp_e_impl::recv(
         recv_mode,
         io_type,
         recv_otw_type, //TODO
-        64e6, //TODO
+        MASTER_CLOCK_RATE,
         boost::bind(&data_transport::get_recv_buff, &_io_impl->transport),
         offsetof(usrp_transfer_frame, buf)
     );
diff --git a/host/lib/usrp/usrp_e/mboard_impl.cpp b/host/lib/usrp/usrp_e/mboard_impl.cpp
index 00ce4b782..efbde38ce 100644
--- a/host/lib/usrp/usrp_e/mboard_impl.cpp
+++ b/host/lib/usrp/usrp_e/mboard_impl.cpp
@@ -108,6 +108,14 @@ void usrp_e_impl::mboard_get(const wax::obj &key_, wax::obj &val){
 /***********************************************************************
  * Mboard Set
  **********************************************************************/
-void usrp_e_impl::mboard_set(const wax::obj &, const wax::obj &){
-    UHD_THROW_PROP_SET_ERROR();
+void usrp_e_impl::mboard_set(const wax::obj &key, const wax::obj &val){
+    //handle the get request conditioned on the key
+    switch(key.as<mboard_prop_t>()){
+
+    case MBOARD_PROP_STREAM_CMD:
+        issue_stream_cmd(val.as<stream_cmd_t>());
+        return;
+
+    default: UHD_THROW_PROP_SET_ERROR();
+    }
 }
diff --git a/host/lib/usrp/usrp_e/usrp_e_impl.hpp b/host/lib/usrp/usrp_e/usrp_e_impl.hpp
index bdb1a675b..a9fd856fe 100644
--- a/host/lib/usrp/usrp_e/usrp_e_impl.hpp
+++ b/host/lib/usrp/usrp_e/usrp_e_impl.hpp
@@ -22,11 +22,14 @@
 #include <uhd/usrp/usrp_e.hpp>
 #include <uhd/usrp/dboard_eeprom.hpp>
 #include <uhd/types/clock_config.hpp>
+#include <uhd/types/stream_cmd.hpp>
 #include <uhd/usrp/dboard_manager.hpp>
 
 #ifndef INCLUDED_USRP_E_IMPL_HPP
 #define INCLUDED_USRP_E_IMPL_HPP
 
+static const double MASTER_CLOCK_RATE = 64e6;
+
 /*!
  * Make a usrp-e dboard interface.
  * \param iface the usrp-e interface object
@@ -90,6 +93,7 @@ private:
     //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);
 
     //configuration shadows
     uhd::clock_config_t _clock_config;
@@ -131,12 +135,14 @@ private:
     void rx_ddc_init(void);
     void rx_ddc_get(const wax::obj &, wax::obj &);
     void rx_ddc_set(const wax::obj &, const wax::obj &);
+    double _ddc_freq; size_t _ddc_decim;
     wax_obj_proxy::sptr _rx_ddc_proxy;
 
     //tx duc functions and settings
     void tx_duc_init(void);
     void tx_duc_get(const wax::obj &, wax::obj &);
     void tx_duc_set(const wax::obj &, const wax::obj &);
+    double _duc_freq; size_t _duc_interp;
     wax_obj_proxy::sptr _tx_duc_proxy;
 };