1 files changed, 116 insertions, 0 deletions
diff --git a/host/lib/usrp/multi_usrp.cpp b/host/lib/usrp/multi_usrp.cpp
index 1866255c9..f27d0ca03 100644
--- a/host/lib/usrp/multi_usrp.cpp
+++ b/host/lib/usrp/multi_usrp.cpp
@@ -26,6 +26,7 @@
 #include <uhd/usrp/mboard_eeprom.hpp>
 #include <uhd/usrp/dboard_eeprom.hpp>
 #include <uhd/convert.hpp>
+#include <uhd/utils/soft_register.hpp>
 #include <boost/assign/list_of.hpp>
 #include <boost/thread.hpp>
 #include <boost/foreach.hpp>
@@ -1371,6 +1372,121 @@ public:
         return 0;
     }
 
+    void write_register(const std::string &path, const boost::uint32_t field, const boost::uint64_t value, const size_t mboard)
+    {
+        if (_tree->exists(mb_root(mboard) / "registers"))
+        {
+            uhd::soft_regmap_accessor_t::sptr accessor =
+                _tree->access<uhd::soft_regmap_accessor_t::sptr>(mb_root(mboard) / "registers").get();
+            uhd::soft_register_base& reg = accessor->lookup(path);
+
+            if (not reg.is_writable()) {
+                throw uhd::runtime_error("multi_usrp::write_register - register not writable: " + path);
+            }
+
+            switch (reg.get_bitwidth()) {
+            case 16:
+                if (reg.is_readable())
+                    uhd::soft_register_base::cast<uhd::soft_reg16_rw_t>(reg).write(field, static_cast<boost::uint16_t>(value));
+                else
+                    uhd::soft_register_base::cast<uhd::soft_reg16_wo_t>(reg).write(field, static_cast<boost::uint16_t>(value));
+            break;
+
+            case 32:
+                if (reg.is_readable())
+                    uhd::soft_register_base::cast<uhd::soft_reg32_rw_t>(reg).write(field, static_cast<boost::uint32_t>(value));
+                else
+                    uhd::soft_register_base::cast<uhd::soft_reg32_wo_t>(reg).write(field, static_cast<boost::uint32_t>(value));
+            break;
+
+            case 64:
+                if (reg.is_readable())
+                    uhd::soft_register_base::cast<uhd::soft_reg64_rw_t>(reg).write(field, value);
+                else
+                    uhd::soft_register_base::cast<uhd::soft_reg64_wo_t>(reg).write(field, value);
+            break;
+
+            default:
+                throw uhd::assertion_error("multi_usrp::write_register - register has invalid bitwidth");
+            }
+
+        } else {
+            throw uhd::not_implemented_error("multi_usrp::write_register - register IO not supported for this device");
+        }
+    }
+
+    boost::uint64_t read_register(const std::string &path, const boost::uint32_t field, const size_t mboard)
+    {
+        if (_tree->exists(mb_root(mboard) / "registers"))
+        {
+            uhd::soft_regmap_accessor_t::sptr accessor =
+                _tree->access<uhd::soft_regmap_accessor_t::sptr>(mb_root(mboard) / "registers").get();
+            uhd::soft_register_base& reg = accessor->lookup(path);
+
+            if (not reg.is_readable()) {
+                throw uhd::runtime_error("multi_usrp::read_register - register not readable: " + path);
+            }
+
+            switch (reg.get_bitwidth()) {
+            case 16:
+                if (reg.is_writable())
+                    return static_cast<boost::uint64_t>(uhd::soft_register_base::cast<uhd::soft_reg16_rw_t>(reg).read(field));
+                else
+                    return static_cast<boost::uint64_t>(uhd::soft_register_base::cast<uhd::soft_reg16_ro_t>(reg).read(field));
+            break;
+
+            case 32:
+                if (reg.is_writable())
+                    return static_cast<boost::uint64_t>(uhd::soft_register_base::cast<uhd::soft_reg32_rw_t>(reg).read(field));
+                else
+                    return static_cast<boost::uint64_t>(uhd::soft_register_base::cast<uhd::soft_reg32_ro_t>(reg).read(field));
+            break;
+
+            case 64:
+                if (reg.is_writable())
+                    return uhd::soft_register_base::cast<uhd::soft_reg64_rw_t>(reg).read(field);
+                else
+                    return uhd::soft_register_base::cast<uhd::soft_reg64_ro_t>(reg).read(field);
+            break;
+
+            default:
+                throw uhd::assertion_error("multi_usrp::read_register - register has invalid bitwidth: " + path);
+            }
+        } else {
+            throw uhd::not_implemented_error("multi_usrp::read_register - register IO not supported for this device");
+        }
+    }
+
+    std::vector<std::string> enumerate_registers(const size_t mboard)
+    {
+        if (_tree->exists(mb_root(mboard) / "registers"))
+        {
+            uhd::soft_regmap_accessor_t::sptr accessor =
+                _tree->access<uhd::soft_regmap_accessor_t::sptr>(mb_root(mboard) / "registers").get();
+            return accessor->enumerate();
+        } else {
+            return std::vector<std::string>();
+        }
+    }
+
+    register_info_t get_register_info(const std::string &path, const size_t mboard = 0)
+    {
+        if (_tree->exists(mb_root(mboard) / "registers"))
+        {
+            uhd::soft_regmap_accessor_t::sptr accessor =
+                _tree->access<uhd::soft_regmap_accessor_t::sptr>(mb_root(mboard) / "registers").get();
+            uhd::soft_register_base& reg = accessor->lookup(path);
+
+            register_info_t info;
+            info.bitwidth = reg.get_bitwidth();
+            info.readable = reg.is_readable();
+            info.writable = reg.is_writable();
+            return info;
+        } else {
+            throw uhd::not_implemented_error("multi_usrp::read_register - register IO not supported for this device");
+        }
+    }
+
 private:
     device::sptr _dev;
     property_tree::sptr _tree;