cores: Factored out some common code

7 files changed, 121 insertions, 144 deletions

diff --git a/host/lib/usrp/cores/CMakeLists.txt b/host/lib/usrp/cores/CMakeLists.txt
index 720f1d957..404fc6137 100644
--- a/host/lib/usrp/cores/CMakeLists.txt
+++ b/host/lib/usrp/cores/CMakeLists.txt
@@ -37,6 +37,7 @@ LIBUHD_APPEND_SOURCES(
     ${CMAKE_CURRENT_SOURCE_DIR}/time_core_3000.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/spi_core_3000.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/i2c_core_100_wb32.cpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/dsp_core_utils.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/rx_dsp_core_3000.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/tx_dsp_core_3000.cpp
     ${CMAKE_CURRENT_SOURCE_DIR}/radio_ctrl_core_3000.cpp
diff --git a/host/lib/usrp/cores/dsp_core_utils.cpp b/host/lib/usrp/cores/dsp_core_utils.cpp
new file mode 100644
index 000000000..f6f56a16b
--- /dev/null
+++ b/host/lib/usrp/cores/dsp_core_utils.cpp
@@ -0,0 +1,67 @@
+//
+// Copyright 2016 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 "dsp_core_utils.hpp"
+#include <uhd/utils/math.hpp>
+#include <uhd/exception.hpp>
+#include <boost/math/special_functions/round.hpp>
+#include <boost/math/special_functions/sign.hpp>
+
+static const int32_t MAX_FREQ_WORD = boost::numeric::bounds<boost::int32_t>::highest();
+static const int32_t MIN_FREQ_WORD = boost::numeric::bounds<boost::int32_t>::lowest();
+
+void get_freq_and_freq_word(
+        const double requested_freq,
+        const double tick_rate,
+        double &actual_freq,
+        int32_t &freq_word
+) {
+    //correct for outside of rate (wrap around)
+    double freq = std::fmod(requested_freq, tick_rate);
+    if (std::abs(freq) > tick_rate/2.0)
+        freq -= boost::math::sign(freq) * tick_rate;
+
+    //confirm that the target frequency is within range of the CORDIC
+    UHD_ASSERT_THROW(std::abs(freq) <= tick_rate/2.0);
+
+    /* Now calculate the frequency word. It is possible for this calculation
+     * to cause an overflow. As the requested DSP frequency approaches the
+     * master clock rate, that ratio multiplied by the scaling factor (2^32)
+     * will generally overflow within the last few kHz of tunable range.
+     * Thus, we check to see if the operation will overflow before doing it,
+     * and if it will, we set it to the integer min or max of this system.
+     */
+    freq_word = 0;
+
+    static const double scale_factor = std::pow(2.0, 32);
+    if ((freq / tick_rate) >= (MAX_FREQ_WORD / scale_factor)) {
+        /* Operation would have caused a positive overflow of int32. */
+        freq_word = MAX_FREQ_WORD;
+
+    } else if ((freq / tick_rate) <= (MIN_FREQ_WORD / scale_factor)) {
+        /* Operation would have caused a negative overflow of int32. */
+        freq_word = MIN_FREQ_WORD;
+
+    } else {
+        /* The operation is safe. Perform normally. */
+        freq_word = int32_t(boost::math::round((freq / tick_rate) * scale_factor));
+    }
+
+    //program the frequency word into the device DSP
+    actual_freq = (double(freq_word) / scale_factor) * tick_rate;
+}
+
diff --git a/host/lib/usrp/cores/dsp_core_utils.hpp b/host/lib/usrp/cores/dsp_core_utils.hpp
new file mode 100644
index 000000000..5d142f5bb
--- /dev/null
+++ b/host/lib/usrp/cores/dsp_core_utils.hpp
@@ -0,0 +1,33 @@
+//
+// Copyright 2016 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_DSP_CORE_UTILS_HPP
+#define INCLUDED_LIBUHD_DSP_CORE_UTILS_HPP
+
+#include <uhd/types/stdint.hpp>
+
+/*! For a requested frequency and sampling rate, return the
+ *  correct frequency word (to set the CORDIC) and the actual frequency.
+ */
+void get_freq_and_freq_word(
+        const double requested_freq,
+        const double tick_rate,
+        double &actual_freq,
+        int32_t &freq_word
+);
+
+#endif /* INCLUDED_LIBUHD_DSP_CORE_UTILS_HPP */
diff --git a/host/lib/usrp/cores/rx_dsp_core_200.cpp b/host/lib/usrp/cores/rx_dsp_core_200.cpp
index b899085c0..e51862d3b 100644
--- a/host/lib/usrp/cores/rx_dsp_core_200.cpp
+++ b/host/lib/usrp/cores/rx_dsp_core_200.cpp
@@ -16,6 +16,7 @@
 //
 
 #include "rx_dsp_core_200.hpp"
+#include "dsp_core_utils.hpp"
 #include <uhd/types/dict.hpp>
 #include <uhd/exception.hpp>
 #include <uhd/utils/math.hpp>
@@ -24,7 +25,6 @@
 #include <boost/assign/list_of.hpp>
 #include <boost/thread/thread.hpp> //thread sleep
 #include <boost/math/special_functions/round.hpp>
-#include <boost/math/special_functions/sign.hpp>
 #include <boost/numeric/conversion/bounds.hpp>
 #include <algorithm>
 #include <cmath>
@@ -223,42 +223,11 @@ public:
         return _fxpt_scalar_correction*_host_extra_scaling/32767.;
     }
 
-    double set_freq(const double freq_){
-        //correct for outside of rate (wrap around)
-        double freq = std::fmod(freq_, _tick_rate);
-        if (std::abs(freq) > _tick_rate/2.0)
-            freq -= boost::math::sign(freq)*_tick_rate;
-
-        //confirm that the target frequency is within range of the CORDIC
-        UHD_ASSERT_THROW(std::abs(freq) <= _tick_rate/2.0);
-
-        /* Now calculate the frequency word. It is possible for this calculation
-         * to cause an overflow. As the requested DSP frequency approaches the
-         * master clock rate, that ratio multiplied by the scaling factor (2^32)
-         * will generally overflow within the last few kHz of tunable range.
-         * Thus, we check to see if the operation will overflow before doing it,
-         * and if it will, we set it to the integer min or max of this system.
-         */
-        boost::int32_t freq_word = 0;
-
-        static const double scale_factor = std::pow(2.0, 32);
-        if((freq / _tick_rate) >= (uhd::math::BOOST_INT32_MAX / scale_factor)) {
-            /* Operation would have caused a positive overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MAX;
-
-        } else if((freq / _tick_rate) <= (uhd::math::BOOST_INT32_MIN / scale_factor)) {
-            /* Operation would have caused a negative overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MIN;
-
-        } else {
-            /* The operation is safe. Perform normally. */
-            freq_word = boost::int32_t(boost::math::round((freq / _tick_rate) * scale_factor));
-        }
-
-        //program the frequency word into the device DSP
-        const double actual_freq = (double(freq_word) / scale_factor) * _tick_rate;
+    double set_freq(const double requested_freq){
+        double actual_freq;
+        int32_t freq_word;
+        get_freq_and_freq_word(requested_freq, _tick_rate, actual_freq, freq_word);
         _iface->poke32(REG_DSP_RX_FREQ, boost::uint32_t(freq_word));
-
         return actual_freq;
     }
 
diff --git a/host/lib/usrp/cores/rx_dsp_core_3000.cpp b/host/lib/usrp/cores/rx_dsp_core_3000.cpp
index 035bc6a3f..27e55ac51 100644
--- a/host/lib/usrp/cores/rx_dsp_core_3000.cpp
+++ b/host/lib/usrp/cores/rx_dsp_core_3000.cpp
@@ -16,6 +16,7 @@
 //
 
 #include "rx_dsp_core_3000.hpp"
+#include "dsp_core_utils.hpp"
 #include <uhd/types/dict.hpp>
 #include <uhd/exception.hpp>
 #include <uhd/utils/math.hpp>
@@ -24,7 +25,6 @@
 #include <boost/assign/list_of.hpp>
 #include <boost/thread/thread.hpp> //thread sleep
 #include <boost/math/special_functions/round.hpp>
-#include <boost/math/special_functions/sign.hpp>
 #include <algorithm>
 #include <cmath>
 
@@ -209,42 +209,11 @@ public:
         return _fxpt_scalar_correction*_host_extra_scaling/32767.;
     }
 
-    double set_freq(const double freq_){
-        //correct for outside of rate (wrap around)
-        double freq = std::fmod(freq_, _tick_rate);
-        if (std::abs(freq) > _tick_rate/2.0)
-            freq -= boost::math::sign(freq)*_tick_rate;
-
-        //confirm that the target frequency is within range of the CORDIC
-        UHD_ASSERT_THROW(std::abs(freq) <= _tick_rate/2.0);
-
-        /* Now calculate the frequency word. It is possible for this calculation
-         * to cause an overflow. As the requested DSP frequency approaches the
-         * master clock rate, that ratio multiplied by the scaling factor (2^32)
-         * will generally overflow within the last few kHz of tunable range.
-         * Thus, we check to see if the operation will overflow before doing it,
-         * and if it will, we set it to the integer min or max of this system.
-         */
-        boost::int32_t freq_word = 0;
-
-        static const double scale_factor = std::pow(2.0, 32);
-        if((freq / _tick_rate) >= (uhd::math::BOOST_INT32_MAX / scale_factor)) {
-            /* Operation would have caused a positive overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MAX;
-
-        } else if((freq / _tick_rate) <= (uhd::math::BOOST_INT32_MIN / scale_factor)) {
-            /* Operation would have caused a negative overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MIN;
-
-        } else {
-            /* The operation is safe. Perform normally. */
-            freq_word = boost::int32_t(boost::math::round((freq / _tick_rate) * scale_factor));
-        }
-
-        //program the frequency word into the device DSP
-        const double actual_freq = (double(freq_word) / scale_factor) * _tick_rate;
+    double set_freq(const double requested_freq){
+        double actual_freq;
+        int32_t freq_word;
+        get_freq_and_freq_word(requested_freq, _tick_rate, actual_freq, freq_word);
         _iface->poke32(REG_DSP_RX_FREQ, boost::uint32_t(freq_word));
-
         return actual_freq;
     }
 
diff --git a/host/lib/usrp/cores/tx_dsp_core_200.cpp b/host/lib/usrp/cores/tx_dsp_core_200.cpp
index 2ef9f4406..4c456a10d 100644
--- a/host/lib/usrp/cores/tx_dsp_core_200.cpp
+++ b/host/lib/usrp/cores/tx_dsp_core_200.cpp
@@ -16,13 +16,13 @@
 //
 
 #include "tx_dsp_core_200.hpp"
+#include "dsp_core_utils.hpp"
 #include <uhd/types/dict.hpp>
 #include <uhd/exception.hpp>
 #include <uhd/utils/math.hpp>
 #include <uhd/utils/msg.hpp>
 #include <boost/assign/list_of.hpp>
 #include <boost/math/special_functions/round.hpp>
-#include <boost/math/special_functions/sign.hpp>
 #include <boost/thread/thread.hpp> //sleep
 #include <algorithm>
 #include <cmath>
@@ -163,42 +163,11 @@ public:
         return _fxpt_scalar_correction*_host_extra_scaling*32767.;
     }
 
-    double set_freq(const double freq_){
-        //correct for outside of rate (wrap around)
-        double freq = std::fmod(freq_, _tick_rate);
-        if (std::abs(freq) > _tick_rate/2.0)
-            freq -= boost::math::sign(freq)*_tick_rate;
-
-        //confirm that the target frequency is within range of the CORDIC
-        UHD_ASSERT_THROW(std::abs(freq) <= _tick_rate/2.0);
-
-        /* Now calculate the frequency word. It is possible for this calculation
-         * to cause an overflow. As the requested DSP frequency approaches the
-         * master clock rate, that ratio multiplied by the scaling factor (2^32)
-         * will generally overflow within the last few kHz of tunable range.
-         * Thus, we check to see if the operation will overflow before doing it,
-         * and if it will, we set it to the integer min or max of this system.
-         */
-        boost::int32_t freq_word = 0;
-
-        static const double scale_factor = std::pow(2.0, 32);
-        if((freq / _tick_rate) >= (uhd::math::BOOST_INT32_MAX / scale_factor)) {
-            /* Operation would have caused a positive overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MAX;
-
-        } else if((freq / _tick_rate) <= (uhd::math::BOOST_INT32_MIN / scale_factor)) {
-            /* Operation would have caused a negative overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MIN;
-
-        } else {
-            /* The operation is safe. Perform normally. */
-            freq_word = boost::int32_t(boost::math::round((freq / _tick_rate) * scale_factor));
-        }
-
-        //program the frequency word into the device DSP
-        const double actual_freq = (double(freq_word) / scale_factor) * _tick_rate;
+    double set_freq(const double requested_freq){
+        double actual_freq;
+        int32_t freq_word;
+        get_freq_and_freq_word(requested_freq, _tick_rate, actual_freq, freq_word);
         _iface->poke32(REG_DSP_TX_FREQ, boost::uint32_t(freq_word));
-
         return actual_freq;
     }
 
diff --git a/host/lib/usrp/cores/tx_dsp_core_3000.cpp b/host/lib/usrp/cores/tx_dsp_core_3000.cpp
index 7e447ae7d..66bcfb6ea 100644
--- a/host/lib/usrp/cores/tx_dsp_core_3000.cpp
+++ b/host/lib/usrp/cores/tx_dsp_core_3000.cpp
@@ -16,13 +16,13 @@
 //
 
 #include "tx_dsp_core_3000.hpp"
+#include "dsp_core_utils.hpp"
 #include <uhd/types/dict.hpp>
 #include <uhd/exception.hpp>
 #include <uhd/utils/math.hpp>
 #include <uhd/utils/msg.hpp>
 #include <boost/assign/list_of.hpp>
 #include <boost/math/special_functions/round.hpp>
-#include <boost/math/special_functions/sign.hpp>
 #include <boost/thread/thread.hpp> //sleep
 #include <algorithm>
 #include <cmath>
@@ -136,42 +136,11 @@ public:
         return _fxpt_scalar_correction*_host_extra_scaling*32767.;
     }
 
-    double set_freq(const double freq_){
-        //correct for outside of rate (wrap around)
-        double freq = std::fmod(freq_, _tick_rate);
-        if (std::abs(freq) > _tick_rate/2.0)
-            freq -= boost::math::sign(freq)*_tick_rate;
-
-        //confirm that the target frequency is within range of the CORDIC
-        UHD_ASSERT_THROW(std::abs(freq) <= _tick_rate/2.0);
-
-        /* Now calculate the frequency word. It is possible for this calculation
-         * to cause an overflow. As the requested DSP frequency approaches the
-         * master clock rate, that ratio multiplied by the scaling factor (2^32)
-         * will generally overflow within the last few kHz of tunable range.
-         * Thus, we check to see if the operation will overflow before doing it,
-         * and if it will, we set it to the integer min or max of this system.
-         */
-        boost::int32_t freq_word = 0;
-
-        static const double scale_factor = std::pow(2.0, 32);
-        if((freq / _tick_rate) >= (uhd::math::BOOST_INT32_MAX / scale_factor)) {
-            /* Operation would have caused a positive overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MAX;
-
-        } else if((freq / _tick_rate) <= (uhd::math::BOOST_INT32_MIN / scale_factor)) {
-            /* Operation would have caused a negative overflow of int32. */
-            freq_word = uhd::math::BOOST_INT32_MIN;
-
-        } else {
-            /* The operation is safe. Perform normally. */
-            freq_word = boost::int32_t(boost::math::round((freq / _tick_rate) * scale_factor));
-        }
-
-        //program the frequency word into the device DSP
-        const double actual_freq = (double(freq_word) / scale_factor) * _tick_rate;
+    double set_freq(const double requested_freq) {
+        double actual_freq;
+        int32_t freq_word;
+        get_freq_and_freq_word(requested_freq, _tick_rate, actual_freq, freq_word);
         _iface->poke32(REG_DSP_TX_FREQ, boost::uint32_t(freq_word));
-
         return actual_freq;
     }