dsp rework: work on scaling and args parsing on RX and TX dsp

This simplified some copy pasta in the io_impl.cpp files,
and adds a place for sc8 tx mode in the tx dsp core code.

7 files changed, 60 insertions, 38 deletions

diff --git a/host/lib/usrp/b100/io_impl.cpp b/host/lib/usrp/b100/io_impl.cpp
index c98f71754..6e9e017aa 100644
--- a/host/lib/usrp/b100/io_impl.cpp
+++ b/host/lib/usrp/b100/io_impl.cpp
@@ -154,6 +154,8 @@ void b100_impl::update_tx_samp_rate(const size_t dspno, const double rate){
     if (my_streamer.get() == NULL) return;
 
     my_streamer->set_samp_rate(rate);
+    const double adj = _tx_dsp->get_scaling_adjustment();
+    my_streamer->set_scale_factor(adj);
 }
 
 void b100_impl::update_rx_subdev_spec(const uhd::usrp::subdev_spec_t &spec){
@@ -202,7 +204,6 @@ rx_streamer::sptr b100_impl::get_rx_stream(const uhd::stream_args_t &args_){
     //setup defaults for unspecified values
     args.otw_format = args.otw_format.empty()? "sc16" : args.otw_format;
     args.channels = args.channels.empty()? std::vector<size_t>(1, 0) : args.channels;
-    const unsigned sc8_scalar = unsigned(args.args.cast<double>("scalar", 1));
 
     //calculate packet size
     static const size_t hdr_size = 0
@@ -233,8 +234,7 @@ rx_streamer::sptr b100_impl::get_rx_stream(const uhd::stream_args_t &args_){
     for (size_t chan_i = 0; chan_i < args.channels.size(); chan_i++){
         const size_t dsp = args.channels[chan_i];
         _rx_dsps[dsp]->set_nsamps_per_packet(spp); //seems to be a good place to set this
-        if (not args.args.has_key("noclear")) _rx_dsps[dsp]->clear();
-        _rx_dsps[dsp]->set_format(args.otw_format, sc8_scalar);
+        _rx_dsps[dsp]->setup(args);
         my_streamer->set_xport_chan_get_buff(chan_i, boost::bind(
             &recv_packet_demuxer::get_recv_buff, _io_impl->demuxer, dsp, _1
         ), true /*flush*/);
@@ -260,10 +260,6 @@ tx_streamer::sptr b100_impl::get_tx_stream(const uhd::stream_args_t &args_){
     args.otw_format = args.otw_format.empty()? "sc16" : args.otw_format;
     args.channels = args.channels.empty()? std::vector<size_t>(1, 0) : args.channels;
 
-    if (args.otw_format != "sc16"){
-        throw uhd::value_error("USRP TX cannot handle requested wire format: " + args.otw_format);
-    }
-
     //calculate packet size
     static const size_t hdr_size = 0
         + vrt::max_if_hdr_words32*sizeof(boost::uint32_t)
@@ -291,8 +287,7 @@ tx_streamer::sptr b100_impl::get_tx_stream(const uhd::stream_args_t &args_){
     for (size_t chan_i = 0; chan_i < args.channels.size(); chan_i++){
         const size_t dsp = args.channels[chan_i];
         UHD_ASSERT_THROW(dsp == 0); //always 0
-        if (not args.args.has_key("noclear")) _tx_dsp->clear();
-        if (args.args.has_key("underflow_policy")) _tx_dsp->set_underflow_policy(args.args["underflow_policy"]);
+        _tx_dsp->setup(args);
         my_streamer->set_xport_chan_get_buff(chan_i, boost::bind(
             &zero_copy_if::get_send_buff, _data_transport, _1
         ));
diff --git a/host/lib/usrp/cores/rx_dsp_core_200.cpp b/host/lib/usrp/cores/rx_dsp_core_200.cpp
index cbc732c9c..b2bf934bc 100644
--- a/host/lib/usrp/cores/rx_dsp_core_200.cpp
+++ b/host/lib/usrp/cores/rx_dsp_core_200.cpp
@@ -224,17 +224,19 @@ public:
         if (_continuous_streaming) issue_stream_command(stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
     }
 
-    void set_format(const std::string &format, const unsigned scale){
+    void setup(const uhd::stream_args_t &stream_args){
+        if (not stream_args.args.has_key("noclear")) this->clear();
+
         unsigned format_word = 0;
-        if (format == "sc16"){
+        if (stream_args.otw_format == "sc16"){
             format_word = 0;
             _extra_scaling = 1.0;
         }
-        else if (format == "sc8"){
+        else if (stream_args.otw_format == "sc8"){
             format_word = (1 << 0);
-            _extra_scaling = scale;
+            _extra_scaling = stream_args.args.cast<double>("scalar", 1.0);
         }
-        else throw uhd::value_error("USRP RX cannot handle requested wire format: " + format);
+        else throw uhd::value_error("USRP RX cannot handle requested wire format: " + stream_args.otw_format);
 
         this->update_scalar();
 
diff --git a/host/lib/usrp/cores/rx_dsp_core_200.hpp b/host/lib/usrp/cores/rx_dsp_core_200.hpp
index 58be51eee..b01f751e9 100644
--- a/host/lib/usrp/cores/rx_dsp_core_200.hpp
+++ b/host/lib/usrp/cores/rx_dsp_core_200.hpp
@@ -19,6 +19,7 @@
 #define INCLUDED_LIBUHD_USRP_RX_DSP_CORE_200_HPP
 
 #include <uhd/config.hpp>
+#include <uhd/stream.hpp>
 #include <uhd/types/ranges.hpp>
 #include <boost/utility.hpp>
 #include <boost/shared_ptr.hpp>
@@ -60,7 +61,7 @@ public:
 
     virtual void handle_overflow(void) = 0;
 
-    virtual void set_format(const std::string &format, const unsigned scale) = 0;
+    virtual void setup(const uhd::stream_args_t &stream_args) = 0;
 };
 
 #endif /* INCLUDED_LIBUHD_USRP_RX_DSP_CORE_200_HPP */
diff --git a/host/lib/usrp/cores/tx_dsp_core_200.cpp b/host/lib/usrp/cores/tx_dsp_core_200.cpp
index f4c303d05..2c5b86bfc 100644
--- a/host/lib/usrp/cores/tx_dsp_core_200.cpp
+++ b/host/lib/usrp/cores/tx_dsp_core_200.cpp
@@ -58,6 +58,11 @@ public:
     ):
         _iface(iface), _dsp_base(dsp_base), _ctrl_base(ctrl_base), _sid(sid)
     {
+        //init to something so update method has reasonable defaults
+        _scaling_adjustment = 1.0;
+        _extra_scaling = 1.0;
+        _fxpt_scalar_correction = 1.0;
+
         //init the tx control registers
         this->clear();
         this->set_underflow_policy("next_packet");
@@ -121,13 +126,24 @@ public:
 
         // Calculate CIC interpolation (i.e., without halfband interpolators)
         // Calculate closest multiplier constant to reverse gain absent scale multipliers
-        double rate_cubed = std::pow(double(interp & 0xff), 3);
-        const boost::int32_t scale = boost::math::iround((4096*std::pow(2, ceil_log2(rate_cubed)))/(1.65*rate_cubed));
-        _iface->poke32(REG_DSP_TX_SCALE_IQ, scale);
+        const double rate_pow = std::pow(double(interp & 0xff), 3);
+        _scaling_adjustment = std::pow(2, ceil_log2(rate_pow))/(1.65*rate_pow);
+        this->update_scalar();
 
         return _tick_rate/interp_rate;
     }
 
+    void update_scalar(void){
+        const double target_scalar = (1 << 17)*_scaling_adjustment/_extra_scaling;
+        const boost::int32_t actual_scalar = boost::math::iround(target_scalar);
+        _fxpt_scalar_correction = target_scalar/actual_scalar; //should be small
+        _iface->poke32(REG_DSP_TX_SCALE_IQ, actual_scalar);
+    }
+
+    double get_scaling_adjustment(void){
+        return _fxpt_scalar_correction*_extra_scaling*32767.;
+    }
+
     double set_freq(const double freq_){
         //correct for outside of rate (wrap around)
         double freq = std::fmod(freq_, _tick_rate);
@@ -156,10 +172,26 @@ public:
         _iface->poke32(REG_TX_CTRL_PACKETS_PER_UP, (packets_per_up == 0)? 0 : (FLAG_TX_CTRL_UP_ENB | packets_per_up));
     }
 
+    void setup(const uhd::stream_args_t &stream_args){
+        if (not stream_args.args.has_key("noclear")) this->clear();
+
+        if (stream_args.otw_format == "sc16"){
+            _extra_scaling = 1.0;
+        }
+        else throw uhd::value_error("USRP TX cannot handle requested wire format: " + stream_args.otw_format);
+
+        this->update_scalar();
+
+        if (stream_args.args.has_key("underflow_policy")){
+            this->set_underflow_policy(stream_args.args["underflow_policy"]);
+        }
+    }
+
 private:
     wb_iface::sptr _iface;
     const size_t _dsp_base, _ctrl_base;
     double _tick_rate, _link_rate;
+    double _scaling_adjustment, _extra_scaling, _fxpt_scalar_correction;
     const boost::uint32_t _sid;
 };
 
diff --git a/host/lib/usrp/cores/tx_dsp_core_200.hpp b/host/lib/usrp/cores/tx_dsp_core_200.hpp
index 4b39a5b07..0e1cfb6bc 100644
--- a/host/lib/usrp/cores/tx_dsp_core_200.hpp
+++ b/host/lib/usrp/cores/tx_dsp_core_200.hpp
@@ -19,6 +19,7 @@
 #define INCLUDED_LIBUHD_USRP_TX_DSP_CORE_200_HPP
 
 #include <uhd/config.hpp>
+#include <uhd/stream.hpp>
 #include <uhd/types/ranges.hpp>
 #include <boost/utility.hpp>
 #include <boost/shared_ptr.hpp>
@@ -44,14 +45,15 @@ public:
 
     virtual uhd::meta_range_t get_host_rates(void) = 0;
 
+    virtual double get_scaling_adjustment(void) = 0;
+
     virtual uhd::meta_range_t get_freq_range(void) = 0;
 
     virtual double set_freq(const double freq) = 0;
 
     virtual void set_updates(const size_t cycles_per_up, const size_t packets_per_up) = 0;
 
-    virtual void set_underflow_policy(const std::string &policy) = 0;
-
+    virtual void setup(const uhd::stream_args_t &stream_args) = 0;
 };
 
 #endif /* INCLUDED_LIBUHD_USRP_TX_DSP_CORE_200_HPP */
diff --git a/host/lib/usrp/e100/io_impl.cpp b/host/lib/usrp/e100/io_impl.cpp
index 97f4db6f1..89a4ed00b 100644
--- a/host/lib/usrp/e100/io_impl.cpp
+++ b/host/lib/usrp/e100/io_impl.cpp
@@ -217,6 +217,8 @@ void e100_impl::update_tx_samp_rate(const size_t dspno, const double rate){
     if (my_streamer.get() == NULL) return;
 
     my_streamer->set_samp_rate(rate);
+    const double adj = _tx_dsp->get_scaling_adjustment();
+    my_streamer->set_scale_factor(adj);
 }
 
 void e100_impl::update_rates(void){
@@ -278,7 +280,6 @@ rx_streamer::sptr e100_impl::get_rx_stream(const uhd::stream_args_t &args_){
     //setup defaults for unspecified values
     args.otw_format = args.otw_format.empty()? "sc16" : args.otw_format;
     args.channels = args.channels.empty()? std::vector<size_t>(1, 0) : args.channels;
-    const unsigned sc8_scalar = unsigned(args.args.cast<double>("scalar", 1));
 
     //calculate packet size
     static const size_t hdr_size = 0
@@ -309,8 +310,7 @@ rx_streamer::sptr e100_impl::get_rx_stream(const uhd::stream_args_t &args_){
     for (size_t chan_i = 0; chan_i < args.channels.size(); chan_i++){
         const size_t dsp = args.channels[chan_i];
         _rx_dsps[dsp]->set_nsamps_per_packet(spp); //seems to be a good place to set this
-        if (not args.args.has_key("noclear")) _rx_dsps[dsp]->clear();
-        _rx_dsps[dsp]->set_format(args.otw_format, sc8_scalar);
+        _rx_dsps[dsp]->setup(args);
         my_streamer->set_xport_chan_get_buff(chan_i, boost::bind(
             &recv_packet_demuxer::get_recv_buff, _io_impl->demuxer, dsp, _1
         ), true /*flush*/);
@@ -336,10 +336,6 @@ tx_streamer::sptr e100_impl::get_tx_stream(const uhd::stream_args_t &args_){
     args.otw_format = args.otw_format.empty()? "sc16" : args.otw_format;
     args.channels = args.channels.empty()? std::vector<size_t>(1, 0) : args.channels;
 
-    if (args.otw_format != "sc16"){
-        throw uhd::value_error("USRP TX cannot handle requested wire format: " + args.otw_format);
-    }
-
     //calculate packet size
     static const size_t hdr_size = 0
         + vrt::max_if_hdr_words32*sizeof(boost::uint32_t)
@@ -367,8 +363,7 @@ tx_streamer::sptr e100_impl::get_tx_stream(const uhd::stream_args_t &args_){
     for (size_t chan_i = 0; chan_i < args.channels.size(); chan_i++){
         const size_t dsp = args.channels[chan_i];
         UHD_ASSERT_THROW(dsp == 0); //always 0
-        if (not args.args.has_key("noclear")) _tx_dsp->clear();
-        if (args.args.has_key("underflow_policy")) _tx_dsp->set_underflow_policy(args.args["underflow_policy"]);
+        _tx_dsp->setup(args);
         my_streamer->set_xport_chan_get_buff(chan_i, boost::bind(
             &zero_copy_if::get_send_buff, _data_transport, _1
         ));
diff --git a/host/lib/usrp/usrp2/io_impl.cpp b/host/lib/usrp/usrp2/io_impl.cpp
index 5f5369f7d..58a5df2a5 100644
--- a/host/lib/usrp/usrp2/io_impl.cpp
+++ b/host/lib/usrp/usrp2/io_impl.cpp
@@ -307,6 +307,8 @@ void usrp2_impl::update_tx_samp_rate(const std::string &mb, const size_t dsp, co
     if (my_streamer.get() == NULL) return;
 
     my_streamer->set_samp_rate(rate);
+    const double adj = _mbc[mb].tx_dsp->get_scaling_adjustment();
+    my_streamer->set_scale_factor(adj);
 }
 
 void usrp2_impl::update_rates(void){
@@ -380,7 +382,6 @@ rx_streamer::sptr usrp2_impl::get_rx_stream(const uhd::stream_args_t &args_){
     //setup defaults for unspecified values
     args.otw_format = args.otw_format.empty()? "sc16" : args.otw_format;
     args.channels = args.channels.empty()? std::vector<size_t>(1, 0) : args.channels;
-    const unsigned sc8_scalar = unsigned(args.args.cast<double>("scalar", 1));
 
     //calculate packet size
     static const size_t hdr_size = 0
@@ -416,8 +417,7 @@ rx_streamer::sptr usrp2_impl::get_rx_stream(const uhd::stream_args_t &args_){
             if (chan < num_chan_so_far){
                 const size_t dsp = chan + _mbc[mb].rx_chan_occ - num_chan_so_far;
                 _mbc[mb].rx_dsps[dsp]->set_nsamps_per_packet(spp); //seems to be a good place to set this
-                if (not args.args.has_key("noclear")) _mbc[mb].rx_dsps[dsp]->clear();
-                _mbc[mb].rx_dsps[dsp]->set_format(args.otw_format, sc8_scalar);
+                _mbc[mb].rx_dsps[dsp]->setup(args);
                 my_streamer->set_xport_chan_get_buff(chan_i, boost::bind(
                     &zero_copy_if::get_recv_buff, _mbc[mb].rx_dsp_xports[dsp], _1
                 ), true /*flush*/);
@@ -447,10 +447,6 @@ tx_streamer::sptr usrp2_impl::get_tx_stream(const uhd::stream_args_t &args_){
     args.otw_format = args.otw_format.empty()? "sc16" : args.otw_format;
     args.channels = args.channels.empty()? std::vector<size_t>(1, 0) : args.channels;
 
-    if (args.otw_format != "sc16"){
-        throw uhd::value_error("USRP TX cannot handle requested wire format: " + args.otw_format);
-    }
-
     //calculate packet size
     static const size_t hdr_size = 0
         + vrt::max_if_hdr_words32*sizeof(boost::uint32_t)
@@ -485,10 +481,9 @@ tx_streamer::sptr usrp2_impl::get_tx_stream(const uhd::stream_args_t &args_){
             if (chan < num_chan_so_far){
                 const size_t dsp = chan + _mbc[mb].tx_chan_occ - num_chan_so_far;
                 if (not args.args.has_key("noclear")){
-                    _mbc[mb].tx_dsp->clear();
                     _io_impl->fc_mons[abs]->clear();
                 }
-                if (args.args.has_key("underflow_policy")) _mbc[mb].tx_dsp->set_underflow_policy(args.args["underflow_policy"]);
+                _mbc[mb].tx_dsp->setup(args);
                 my_streamer->set_xport_chan_get_buff(chan_i, boost::bind(
                     &usrp2_impl::io_impl::get_send_buff, _io_impl.get(), abs, _1
                 ));