mg: Allow calling set_rate() at runtime

2 files changed, 38 insertions, 6 deletions

diff --git a/host/lib/usrp/dboard/magnesium/magnesium_constants.hpp b/host/lib/usrp/dboard/magnesium/magnesium_constants.hpp
index 151e68220..2e9debaae 100644
--- a/host/lib/usrp/dboard/magnesium/magnesium_constants.hpp
+++ b/host/lib/usrp/dboard/magnesium/magnesium_constants.hpp
@@ -25,6 +25,7 @@ static constexpr double AD9371_MAX_FREQ = 6.0e9; // Hz
 static constexpr double ADF4351_MIN_FREQ = 35.0e6;
 static constexpr double ADF4351_MAX_FREQ = 4.4e9;
 
+static const std::vector<double> MAGNESIUM_RADIO_RATES = {122.88e6, 125e6, 153.6e6};
 static constexpr double MAGNESIUM_RADIO_RATE = 125e6; // Hz
 static constexpr double MAGNESIUM_MIN_FREQ = 1e6; // Hz
 static constexpr double MAGNESIUM_MAX_FREQ = 6e9; // Hz
diff --git a/host/lib/usrp/dboard/magnesium/magnesium_radio_ctrl_impl.cpp b/host/lib/usrp/dboard/magnesium/magnesium_radio_ctrl_impl.cpp
index 04af0c7a4..3fd32bda9 100644
--- a/host/lib/usrp/dboard/magnesium/magnesium_radio_ctrl_impl.cpp
+++ b/host/lib/usrp/dboard/magnesium/magnesium_radio_ctrl_impl.cpp
@@ -126,15 +126,46 @@ magnesium_radio_ctrl_impl::~magnesium_radio_ctrl_impl()
 /******************************************************************************
  * API Calls
  *****************************************************************************/
-double magnesium_radio_ctrl_impl::set_rate(double rate)
+double magnesium_radio_ctrl_impl::set_rate(double requested_rate)
 {
+    meta_range_t rates;
+    for (const double rate : MAGNESIUM_RADIO_RATES) {
+        rates.push_back(range_t(rate));
+    }
+
+    const double rate = rates.clip(requested_rate);
+    if (!math::frequencies_are_equal(requested_rate, rate)) {
+       UHD_LOG_WARNING(unique_id(),
+            "Coercing requested sample rate from " << (requested_rate/1e6) <<
+            " to " << (rate/1e6)
+       );
+    }
+
+    const double current_rate = get_rate();
+    if (math::frequencies_are_equal(current_rate, rate)) {
+       UHD_LOG_DEBUG(unique_id(),
+            "Rate is already at " << rate << ". Skipping set_rate()");
+       return current_rate;
+    }
+
     std::lock_guard<std::mutex> l(_set_lock);
-    // TODO: implement
-    if (rate != get_rate()) {
-        UHD_LOG_WARNING(unique_id(),
-                "Attempting to set sampling rate to invalid value " << rate);
+    // Now commit to device. First, disable LOs.
+    _lo_disable(_tx_lo);
+    _lo_disable(_rx_lo);
+    const double new_rate = _ad9371->set_master_clock_rate(rate);
+    // Frequency settings apply to both channels, no loop needed. Will also
+    // re-enable the lowband LOs if they were used.
+    set_rx_frequency(get_rx_frequency(0), 0);
+    set_tx_frequency(get_tx_frequency(0), 0);
+    // Gain and bandwidth need to be looped:
+    for (size_t radio_idx = 0; radio_idx < _get_num_radios(); radio_idx++) {
+       set_rx_gain(get_rx_gain(radio_idx), radio_idx);
+       set_tx_gain(get_rx_gain(radio_idx), radio_idx);
+       set_rx_bandwidth(get_rx_bandwidth(radio_idx), radio_idx);
+       set_tx_bandwidth(get_tx_bandwidth(radio_idx), radio_idx);
     }
-    return get_rate();
+    radio_ctrl_impl::set_rate(new_rate);
+    return new_rate;
 }
 
 void magnesium_radio_ctrl_impl::set_tx_antenna(