implemented dsp and rx control

1 files changed, 128 insertions, 9 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();
+    }
 }