uhd: math: Replace wrap-frequency math with a single function

In multiple places in the UHD code, we were doing the same calculation
for a wrapped frequency (wrap it into the first Nyquist zone). This math
was using boost::math, too. Instead of editing every instance, we create
a new function, uhd::math::wrap_frequency(), and replace all of its
separate implementations with this function. The new function also no
longer relies on boost::math::sign.

5 files changed, 41 insertions, 21 deletions

diff --git a/host/include/uhd/utils/math.hpp b/host/include/uhd/utils/math.hpp
index 4918ddf2e..6c8fceae9 100644
--- a/host/include/uhd/utils/math.hpp
+++ b/host/include/uhd/utils/math.hpp
@@ -278,6 +278,24 @@ inline constexpr int sign(T x)
     return (T(0) < x) - (x < T(0));
 }
 
+//! Return a wrapped frequency that is the equivalent frequency in the first
+// Nyquist zone.
+//
+// Examples:
+// - Just above the sampling rate:
+//   wrap_frequency(250e6, 200e6) == 50e6
+// - Just outside the Nyquist zone:
+//   wrap_frequency(120e6, 200e6) == -80e6
+// - Also works for negative frequencies:
+//   wrap_frequency(-250e6, 200e6) == -50e6
+inline double wrap_frequency(const double requested_freq, const double rate)
+{
+    double freq = std::fmod(requested_freq, rate);
+    if (std::abs(freq) > rate / 2.0)
+        freq -= uhd::math::sign(freq) * rate;
+    return freq;
+}
+
 } // namespace math
 } // namespace uhd
 
diff --git a/host/lib/usrp/cores/dsp_core_utils.cpp b/host/lib/usrp/cores/dsp_core_utils.cpp
index 84331fcec..868e47234 100644
--- a/host/lib/usrp/cores/dsp_core_utils.cpp
+++ b/host/lib/usrp/cores/dsp_core_utils.cpp
@@ -8,7 +8,6 @@
 #include <uhd/exception.hpp>
 #include <uhd/utils/math.hpp>
 #include <uhdlib/usrp/cores/dsp_core_utils.hpp>
-#include <boost/math/special_functions/sign.hpp>
 #include <cmath>
 #include <limits>
 
@@ -20,10 +19,7 @@ void get_freq_and_freq_word(const double requested_freq,
     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;
+    const double freq = uhd::math::wrap_frequency(requested_freq, tick_rate);
 
     // confirm that the target frequency is within range of the CORDIC
     UHD_ASSERT_THROW(std::abs(freq) <= tick_rate / 2.0);
diff --git a/host/lib/usrp/usrp1/codec_ctrl.cpp b/host/lib/usrp/usrp1/codec_ctrl.cpp
index 413caaf67..c5268b3c4 100644
--- a/host/lib/usrp/usrp1/codec_ctrl.cpp
+++ b/host/lib/usrp/usrp1/codec_ctrl.cpp
@@ -12,13 +12,13 @@
 #include <uhd/utils/algorithm.hpp>
 #include <uhd/utils/byteswap.hpp>
 #include <uhd/utils/log.hpp>
+#include <uhd/utils/math.hpp>
 #include <uhd/utils/safe_call.hpp>
 #include <uhdlib/utils/narrow.hpp>
-#include <cstdint>
 #include <boost/assign/list_of.hpp>
 #include <boost/format.hpp>
-#include <boost/math/special_functions/sign.hpp>
 #include <cmath>
+#include <cstdint>
 #include <iomanip>
 #include <tuple>
 
@@ -380,15 +380,10 @@ double usrp1_codec_ctrl_impl::fine_tune(double codec_rate, double target_freq)
 
 void usrp1_codec_ctrl_impl::set_duc_freq(double freq, double rate)
 {
-    double codec_rate = rate * 2;
-
-    // correct for outside of rate (wrap around)
-    freq = std::fmod(freq, rate);
-    if (std::abs(freq) > rate / 2.0)
-        freq -= boost::math::sign(freq) * rate;
-
-    double coarse_freq = coarse_tune(codec_rate, freq);
-    double fine_freq   = fine_tune(codec_rate / 4, freq - coarse_freq);
+    const double codec_rate  = rate * 2;
+    freq                     = uhd::math::wrap_frequency(freq, rate);
+    const double coarse_freq = coarse_tune(codec_rate, freq);
+    const double fine_freq   = fine_tune(codec_rate / 4, freq - coarse_freq);
 
     UHD_LOGGER_DEBUG("USRP1") << "ad9862 tuning result:"
                               << "   requested:   " << freq
diff --git a/host/lib/usrp/usrp1/io_impl.cpp b/host/lib/usrp/usrp1/io_impl.cpp
index 5d7b4a77b..713dc4c77 100644
--- a/host/lib/usrp/usrp1/io_impl.cpp
+++ b/host/lib/usrp/usrp1/io_impl.cpp
@@ -14,10 +14,10 @@
 #include "../../transport/super_send_packet_handler.hpp"
 #include <uhd/transport/bounded_buffer.hpp>
 #include <uhd/utils/log.hpp>
+#include <uhd/utils/math.hpp>
 #include <uhd/utils/safe_call.hpp>
 #include <uhd/utils/tasks.hpp>
 #include <boost/format.hpp>
-#include <boost/math/special_functions/sign.hpp>
 #include <boost/thread/thread.hpp>
 #include <atomic>
 #include <chrono>
@@ -582,10 +582,13 @@ void usrp1_impl::update_rates(void)
 
 double usrp1_impl::update_rx_dsp_freq(const size_t dspno, const double freq_)
 {
-    // correct for outside of rate (wrap around)
-    double freq = std::fmod(freq_, _master_clock_rate);
-    if (std::abs(freq) > _master_clock_rate / 2.0)
-        freq -= boost::math::sign(freq) * _master_clock_rate;
+    // Note: The calculation of freq and freq_word could be done by
+    // get_freq_and_freq_word(), which has implemented this algorithm for all
+    // other USRPs.
+    // We'll not refactor this because it's ancient code, but note that
+    // get_freq_and_freq_word() fixes a numerical overflow issue that is not
+    // fixed here.
+    const double freq = uhd::math::wrap_frequency(freq_, _master_clock_rate);
 
     // calculate the freq register word (signed)
     UHD_ASSERT_THROW(std::abs(freq) <= _master_clock_rate / 2.0);
diff --git a/host/tests/math_test.cpp b/host/tests/math_test.cpp
index 1fb01f5c1..12ede7f06 100644
--- a/host/tests/math_test.cpp
+++ b/host/tests/math_test.cpp
@@ -24,3 +24,11 @@ BOOST_AUTO_TEST_CASE(test_sign)
     BOOST_CHECK_EQUAL(uhd::math::sign(-2.3), -1);
     BOOST_CHECK_EQUAL(uhd::math::sign(0.0), 0);
 }
+
+BOOST_AUTO_TEST_CASE(test_wrap_frequency)
+{
+    BOOST_CHECK_EQUAL(uhd::math::wrap_frequency(10e6, 200e6), 10e6);
+    BOOST_CHECK_EQUAL(uhd::math::wrap_frequency(250e6, 200e6), 50e6);
+    BOOST_CHECK_EQUAL(uhd::math::wrap_frequency(120e6, 200e6), -80e6);
+    BOOST_CHECK_EQUAL(uhd::math::wrap_frequency(-250e6, 200e6), -50e6);
+}