5 files changed, 150 insertions, 10 deletions
diff --git a/host/lib/usrp/cores/rx_dsp_core_200.hpp b/host/lib/usrp/cores/rx_dsp_core_200.hpp
index dbd7df912..75935bd1e 100644
--- a/host/lib/usrp/cores/rx_dsp_core_200.hpp
+++ b/host/lib/usrp/cores/rx_dsp_core_200.hpp
@@ -23,6 +23,7 @@
 #include <boost/shared_ptr.hpp>
 #include <uhd/types/stream_cmd.hpp>
 #include "wb_iface.hpp"
+#include <string>
 
 class rx_dsp_core_200 : boost::noncopyable{
 public:
diff --git a/host/lib/usrp/cores/rx_frontend_core_200.cpp b/host/lib/usrp/cores/rx_frontend_core_200.cpp
index 28e57a13b..646e1890a 100644
--- a/host/lib/usrp/cores/rx_frontend_core_200.cpp
+++ b/host/lib/usrp/cores/rx_frontend_core_200.cpp
@@ -15,14 +15,47 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 
-/*
-    void set_mux(const std::string &mode){
-        static const uhd::dict<std::string, boost::uint32_t> mode_to_mux = boost::assign::map_list_of
-            ("iq", (0x1 << 4) | (0x0 << 0)) //DAC0Q=DUC0Q, DAC0I=DUC0I
-            ("qi", (0x0 << 4) | (0x1 << 0)) //DAC0Q=DUC0I, DAC0I=DUC0Q
-            ("i",  (0xf << 4) | (0x0 << 0)) //DAC0Q=ZERO,  DAC0I=DUC0I
-            ("q",  (0x0 << 4) | (0xf << 0)) //DAC0Q=DUC0I, DAC0I=ZERO
-        ;
-        _iface->poke32(, mode_to_mux[mode]);
+#include "rx_frontend_core_200.hpp"
+#include <boost/math/special_functions/round.hpp>
+
+#define REG_RX_FE_SWAP_IQ             _base + 0 //lower bit
+#define REG_RX_FE_MAG_CORRECTION      _base + 1 //18 bits
+#define REG_RX_FE_PHASE_CORRECTION    _base + 2 //18 bits
+#define REG_RX_FE_OFFSET_I            _base + 3 //18 bits
+#define REG_RX_FE_OFFSET_Q            _base + 4 //18 bits
+
+static boost::uint32_t fs_to_bits(const double num, const size_t bits){
+    return boost::int32_t(boost::math::round(num * (1 << (bits-1))));
+}
+
+
+class rx_frontend_core_200_impl : public rx_frontend_core_200{
+public:
+    rx_frontend_core_200_impl(wb_iface::sptr iface, const size_t base):
+        _iface(iface), _base(base)
+    {
+        //NOP
+    }
+
+    void set_mux(const bool swap){
+        _iface->poke32(REG_RX_FE_SWAP_IQ, swap? 1 : 0);
     }
-*/
+
+    void set_offset(const std::complex<double> &off){
+        _iface->poke32(REG_RX_FE_OFFSET_I, fs_to_bits(off.real(), 24));
+        _iface->poke32(REG_RX_FE_OFFSET_Q, fs_to_bits(off.imag(), 24));
+    }
+
+    void set_correction(const std::complex<double> &cor){
+        _iface->poke32(REG_RX_FE_MAG_CORRECTION, fs_to_bits(std::abs(cor), 18));
+        _iface->poke32(REG_RX_FE_PHASE_CORRECTION, fs_to_bits(std::atan2(cor.real(), cor.imag()), 18));
+    }
+
+private:
+    wb_iface::sptr _iface;
+    const size_t _base;
+};
+
+rx_frontend_core_200::sptr rx_frontend_core_200::make(wb_iface::sptr iface, const size_t base){
+    return sptr(new rx_frontend_core_200_impl(iface, base));
+}
diff --git a/host/lib/usrp/cores/rx_frontend_core_200.hpp b/host/lib/usrp/cores/rx_frontend_core_200.hpp
index c339a3a94..19fcce884 100644
--- a/host/lib/usrp/cores/rx_frontend_core_200.hpp
+++ b/host/lib/usrp/cores/rx_frontend_core_200.hpp
@@ -14,3 +14,29 @@
 // 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_USRP_TX_FRONTEND_CORE_200_HPP
+#define INCLUDED_LIBUHD_USRP_TX_FRONTEND_CORE_200_HPP
+
+#include <uhd/config.hpp>
+#include <boost/utility.hpp>
+#include <boost/shared_ptr.hpp>
+#include "wb_iface.hpp"
+#include <complex>
+#include <string>
+
+class rx_frontend_core_200 : boost::noncopyable{
+public:
+    typedef boost::shared_ptr<rx_frontend_core_200> sptr;
+
+    sptr make(wb_iface::sptr iface, const size_t base);
+
+    virtual void set_mux(const bool swap) = 0;
+
+    virtual void set_offset(const std::complex<double> &off) = 0;
+
+    virtual void set_correction(const std::complex<double> &cor) = 0;
+
+};
+
+#endif /* INCLUDED_LIBUHD_USRP_TX_FRONTEND_CORE_200_HPP */
diff --git a/host/lib/usrp/cores/tx_frontend_core_200.cpp b/host/lib/usrp/cores/tx_frontend_core_200.cpp
index c339a3a94..682e6613d 100644
--- a/host/lib/usrp/cores/tx_frontend_core_200.cpp
+++ b/host/lib/usrp/cores/tx_frontend_core_200.cpp
@@ -14,3 +14,57 @@
 // 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 "tx_frontend_core_200.hpp"
+#include <uhd/types/dict.hpp>
+#include <uhd/exception.hpp>
+#include <boost/assign/list_of.hpp>
+#include <boost/math/special_functions/round.hpp>
+
+#define REG_TX_FE_DC_OFFSET_I         _base + 0 //24 bits
+#define REG_TX_FE_DC_OFFSET_Q         _base + 1 //24 bits
+#define REG_TX_FE_MAG_CORRECTION      _base + 2 //18 bits
+#define REG_TX_FE_PHASE_CORRECTION    _base + 3 //18 bits
+#define REG_TX_FE_MUX                 _base + 4 //8 bits (std output = 0x10, reversed = 0x01)
+
+static boost::uint32_t fs_to_bits(const double num, const size_t bits){
+    return boost::int32_t(boost::math::round(num * (1 << (bits-1))));
+}
+
+
+class tx_frontend_core_200_impl : public tx_frontend_core_200{
+public:
+    tx_frontend_core_200_impl(wb_iface::sptr iface, const size_t base):
+        _iface(iface), _base(base)
+    {
+        //NOP
+    }
+
+    void set_mux(const std::string &mode){
+        static const uhd::dict<std::string, boost::uint32_t> mode_to_mux = boost::assign::map_list_of
+            ("iq", (0x1 << 4) | (0x0 << 0)) //DAC0Q=DUC0Q, DAC0I=DUC0I
+            ("qi", (0x0 << 4) | (0x1 << 0)) //DAC0Q=DUC0I, DAC0I=DUC0Q
+            ("i",  (0xf << 4) | (0x0 << 0)) //DAC0Q=ZERO,  DAC0I=DUC0I
+            ("q",  (0x0 << 4) | (0xf << 0)) //DAC0Q=DUC0I, DAC0I=ZERO
+        ;
+        _iface->poke32(REG_TX_FE_MUX, mode_to_mux[mode]);
+    }
+
+    void set_dc_offset(const std::complex<double> &off){
+        _iface->poke32(REG_TX_FE_DC_OFFSET_I, fs_to_bits(off.real(), 24));
+        _iface->poke32(REG_TX_FE_DC_OFFSET_Q, fs_to_bits(off.imag(), 24));
+    }
+
+    void set_correction(const std::complex<double> &cor){
+        _iface->poke32(REG_TX_FE_MAG_CORRECTION, fs_to_bits(std::abs(cor), 18));
+        _iface->poke32(REG_TX_FE_PHASE_CORRECTION, fs_to_bits(std::atan2(cor.real(), cor.imag()), 18));
+    }
+
+private:
+    wb_iface::sptr _iface;
+    const size_t _base;
+};
+
+tx_frontend_core_200::sptr tx_frontend_core_200::make(wb_iface::sptr iface, const size_t base){
+    return sptr(new tx_frontend_core_200_impl(iface, base));
+}
diff --git a/host/lib/usrp/cores/tx_frontend_core_200.hpp b/host/lib/usrp/cores/tx_frontend_core_200.hpp
index c339a3a94..c4a06a14a 100644
--- a/host/lib/usrp/cores/tx_frontend_core_200.hpp
+++ b/host/lib/usrp/cores/tx_frontend_core_200.hpp
@@ -14,3 +14,29 @@
 // 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_USRP_RX_FRONTEND_CORE_200_HPP
+#define INCLUDED_LIBUHD_USRP_RX_FRONTEND_CORE_200_HPP
+
+#include <uhd/config.hpp>
+#include <boost/utility.hpp>
+#include <boost/shared_ptr.hpp>
+#include "wb_iface.hpp"
+#include <complex>
+#include <string>
+
+class tx_frontend_core_200 : boost::noncopyable{
+public:
+    typedef boost::shared_ptr<tx_frontend_core_200> sptr;
+
+    sptr make(wb_iface::sptr iface, const size_t base);
+
+    virtual void set_mux(const std::string &mode) = 0;
+
+    virtual void set_dc_offset(const std::complex<double> &off) = 0;
+
+    virtual void set_correction(const std::complex<double> &cor) = 0;
+
+};
+
+#endif /* INCLUDED_LIBUHD_USRP_RX_FRONTEND_CORE_200_HPP */