uhd: Added APIs to multi_usrp to read/write device registers

- Added regmap object to soft_register library
- Added a regmap_db object to hold multiple regmaps/dbs
- Multiple soft_register enhancements

3 files changed, 454 insertions, 24 deletions

diff --git a/host/include/uhd/usrp/multi_usrp.hpp b/host/include/uhd/usrp/multi_usrp.hpp
index 8d5dc2e5f..0c1e73895 100644
--- a/host/include/uhd/usrp/multi_usrp.hpp
+++ b/host/include/uhd/usrp/multi_usrp.hpp
@@ -975,6 +975,35 @@ public:
     virtual boost::uint32_t get_gpio_attr(const std::string &bank, const std::string &attr, const size_t mboard = 0) = 0;
 
     /*******************************************************************
+     * Register IO methods
+     ******************************************************************/
+
+    /*!
+     * Write a low-level register field for a register in the USRP hardware
+     * \param path the full path to the register
+     * \param field the identifier of bitfield to be written (all other bits remain unchanged)
+     * \param value the value to write to the register field
+     * \param mboard the motherboard index 0 to M-1
+     */
+    virtual void write_register(const std::string &path, const boost::uint32_t field, const boost::uint64_t value, const size_t mboard = 0) = 0;
+
+    /*!
+     * Read a low-level register field from a register in the USRP hardware
+     * \param path the full path to the register
+     * \param field the identifier of bitfield to be read
+     * \param mboard the motherboard index 0 to M-1
+     * \return the value of the register field
+     */
+    virtual boost::uint64_t read_register(const std::string &path, const boost::uint32_t field, const size_t mboard = 0) = 0;
+
+    /*!
+     * Enumerate the full paths of all low-level USRP register accessible to read/write
+     * \param mboard the motherboard index 0 to M-1
+     * \return a vector of register paths
+     */
+    virtual std::vector<std::string> enumerate_registers(const size_t mboard = 0) = 0;
+
+    /*******************************************************************
      * Filter API methods
      ******************************************************************/
 
diff --git a/host/include/uhd/utils/soft_register.hpp b/host/include/uhd/utils/soft_register.hpp
index d3537a618..f28b2aa2a 100644
--- a/host/include/uhd/utils/soft_register.hpp
+++ b/host/include/uhd/utils/soft_register.hpp
@@ -24,6 +24,15 @@
 #include <uhd/exception.hpp>
 #include <boost/thread/mutex.hpp>
 #include <boost/thread/locks.hpp>
+#include <boost/unordered_map.hpp>
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/lexical_cast.hpp>
+#include <boost/lockfree/detail/branch_hints.hpp>
+
+//==================================================================
+// Soft Register Definition
+//==================================================================
 
 #define UHD_DEFINE_SOFT_REG_FIELD(name, width, shift) \
     static const uhd::soft_reg_field_t name = (((shift & 0xFF) << 8) | (width & 0xFF))
@@ -49,19 +58,54 @@ namespace soft_reg_field {
 
     template<typename data_t>
     inline size_t mask(const soft_reg_field_t field) {
-        return ((static_cast<data_t>(1)<<width(field))-1)<<shift(field);
+        static const data_t ONE = static_cast<data_t>(1);
+        //Behavior for the left shit operation is undefined in C++
+        //if the shift amount is >= bitwidth of the datatype
+        //So we treat that as a special case with a branch predicition hint
+        if (boost::lockfree::detail::likely((sizeof(data_t)*8) != width(field)))
+            return ((ONE<<width(field))-ONE)<<shift(field);
+        else
+            return (0-ONE)<<shift(field);
     }
 }
 
+class soft_register_base : public boost::noncopyable {
+public:
+    virtual ~soft_register_base() {}
+
+    virtual void initialize(wb_iface& iface, bool sync = false) = 0;
+    virtual void flush() = 0;
+    virtual void refresh() = 0;
+    virtual size_t get_bitwidth() = 0;
+    virtual bool is_readable() = 0;
+    virtual bool is_writable() = 0;
+
+    /*!
+     * Cast the soft_register generic reference to a more specific type
+     */
+    template <typename soft_reg_t>
+    inline static soft_reg_t& cast(soft_register_base& reg) {
+        soft_reg_t* ptr = dynamic_cast<soft_reg_t*>(&reg);
+        if (ptr) {
+            return *ptr;
+        } else {
+            throw uhd::type_error("failed to cast register to specified type");
+        }
+    }
+};
+
 /*!
  * Soft register object that holds offset, soft-copy and the control iface.
  * Methods give convenient field-level access to soft-copy and the ability
  * to do read-modify-write operations.
  */
-template<typename reg_data_t, bool readable, bool writeable>
-class UHD_API soft_register_t : public boost::noncopyable {
+template<typename reg_data_t, bool readable, bool writable>
+class UHD_API soft_register_t : public soft_register_base {
 public:
-    typedef boost::shared_ptr< soft_register_t<reg_data_t, readable, writeable> > sptr;
+    typedef boost::shared_ptr< soft_register_t<reg_data_t, readable, writable> > sptr;
+
+    //Reserved field. Represents all bits in the register.
+    UHD_DEFINE_SOFT_REG_FIELD(REGISTER, sizeof(reg_data_t)*8, 0);  //[WIDTH-1:0]
 
     /*!
      * Generic constructor for all soft_register types
@@ -88,7 +132,7 @@ public:
         _iface = &iface;
 
         //Synchronize with hardware. For RW register, flush THEN refresh.
-        if (sync && writeable) flush();
+        if (sync && writable) flush();
         if (sync && readable) refresh();
     }
 
@@ -117,12 +161,12 @@ public:
      */
     inline void flush()
     {
-        if (writeable && _iface) {
-            if (sizeof(reg_data_t) <= 2) {
+        if (writable && _iface) {
+            if (get_bitwidth() <= 16) {
                 _iface->poke16(_wr_addr, static_cast<boost::uint16_t>(_soft_copy));
-            } else if (sizeof(reg_data_t) <= 4) {
+            } else if (get_bitwidth() <= 32) {
                 _iface->poke32(_wr_addr, static_cast<boost::uint32_t>(_soft_copy));
-            } else if (sizeof(reg_data_t) <= 8) {
+            } else if (get_bitwidth() <= 64) {
                 _iface->poke64(_wr_addr, static_cast<boost::uint64_t>(_soft_copy));
             } else {
                 throw uhd::not_implemented_error("soft_register only supports up to 64 bits.");
@@ -138,11 +182,11 @@ public:
     inline void refresh()
     {
         if (readable && _iface) {
-            if (sizeof(reg_data_t) <= 2) {
+            if (get_bitwidth() <= 16) {
                 _soft_copy = static_cast<reg_data_t>(_iface->peek16(_rd_addr));
-            } else if (sizeof(reg_data_t) <= 4) {
+            } else if (get_bitwidth() <= 32) {
                 _soft_copy = static_cast<reg_data_t>(_iface->peek32(_rd_addr));
-            } else if (sizeof(reg_data_t) <= 8) {
+            } else if (get_bitwidth() <= 64) {
                 _soft_copy = static_cast<reg_data_t>(_iface->peek64(_rd_addr));
             } else {
                 throw uhd::not_implemented_error("soft_register only supports up to 64 bits.");
@@ -170,6 +214,31 @@ public:
         return get(field);
     }
 
+    /*!
+     * Get bitwidth for this register
+     */
+    inline size_t get_bitwidth()
+    {
+        static const size_t BITS_IN_BYTE = 8;
+        return sizeof(reg_data_t) * BITS_IN_BYTE;
+    }
+
+    /*!
+     * Is the register readable?
+     */
+    inline bool is_readable()
+    {
+        return readable;
+    }
+
+    /*!
+     * Is the register writable?
+     */
+    inline bool is_writable()
+    {
+        return writable;
+    }
+
 private:
     wb_iface*                       _iface;
     const wb_iface::wb_addr_type    _wr_addr;
@@ -181,59 +250,59 @@ private:
  * A synchronized soft register object.
  * All operations in the synchronized register are serialized.
  */
-template<typename reg_data_t, bool readable, bool writeable>
-class UHD_API soft_register_sync_t : public soft_register_t<reg_data_t, readable, writeable> {
+template<typename reg_data_t, bool readable, bool writable>
+class UHD_API soft_register_sync_t : public soft_register_t<reg_data_t, readable, writable> {
 public:
-    typedef boost::shared_ptr< soft_register_sync_t<reg_data_t, readable, writeable> > sptr;
+    typedef boost::shared_ptr< soft_register_sync_t<reg_data_t, readable, writable> > sptr;
 
     soft_register_sync_t(wb_iface::wb_addr_type wr_addr, wb_iface::wb_addr_type rd_addr):
-        soft_register_t<reg_data_t, readable, writeable>(wr_addr, rd_addr), _mutex()
+        soft_register_t<reg_data_t, readable, writable>(wr_addr, rd_addr), _mutex()
     {}
 
     soft_register_sync_t(wb_iface::wb_addr_type addr):
-        soft_register_t<reg_data_t, readable, writeable>(addr), _mutex()
+        soft_register_t<reg_data_t, readable, writable>(addr), _mutex()
     {}
 
     inline void initialize(wb_iface& iface, bool sync = false)
     {
         boost::lock_guard<boost::mutex> lock(_mutex);
-        soft_register_t<reg_data_t, readable, writeable>::initialize(iface, sync);
+        soft_register_t<reg_data_t, readable, writable>::initialize(iface, sync);
     }
 
     inline void set(const soft_reg_field_t field, const reg_data_t value)
     {
         boost::lock_guard<boost::mutex> lock(_mutex);
-        soft_register_t<reg_data_t, readable, writeable>::set(field, value);
+        soft_register_t<reg_data_t, readable, writable>::set(field, value);
     }
 
     inline reg_data_t get(const soft_reg_field_t field)
     {
         boost::lock_guard<boost::mutex> lock(_mutex);
-        return soft_register_t<reg_data_t, readable, writeable>::get(field);
+        return soft_register_t<reg_data_t, readable, writable>::get(field);
     }
 
     inline void flush()
     {
         boost::lock_guard<boost::mutex> lock(_mutex);
-        soft_register_t<reg_data_t, readable, writeable>::flush();
+        soft_register_t<reg_data_t, readable, writable>::flush();
     }
 
     inline void refresh()
     {
         boost::lock_guard<boost::mutex> lock(_mutex);
-        soft_register_t<reg_data_t, readable, writeable>::refresh();
+        soft_register_t<reg_data_t, readable, writable>::refresh();
     }
 
     inline void write(const soft_reg_field_t field, const reg_data_t value)
     {
         boost::lock_guard<boost::mutex> lock(_mutex);
-        soft_register_t<reg_data_t, readable, writeable>::write(field, value);
+        soft_register_t<reg_data_t, readable, writable>::write(field, value);
     }
 
     inline reg_data_t read(const soft_reg_field_t field)
     {
         boost::lock_guard<boost::mutex> lock(_mutex);
-        return soft_register_t<reg_data_t, readable, writeable>::read(field);
+        return soft_register_t<reg_data_t, readable, writable>::read(field);
     }
 
 private:
@@ -306,6 +375,248 @@ typedef soft_register_sync_t<boost::uint64_t, true, true>   soft_reg64_rw_sync_t
     reg_obj->write(example_reg_t::FIELD2, 0x1234);
   }
 */
+}
+
+//==================================================================
+// Soft Register Map and Database Definition
+//==================================================================
+
+namespace uhd {
+
+class UHD_API soft_regmap_accessor_t {
+public:
+    typedef boost::shared_ptr<soft_regmap_accessor_t> sptr;
+
+    virtual ~soft_regmap_accessor_t() {};
+    virtual soft_register_base& lookup(const std::string& path) const = 0;
+    virtual std::vector<std::string> enumerate() const = 0;
+    virtual const std::string& get_name() const = 0;
+};
+
+// A regmap is a collection of registers that share the same
+// bus (control iface). A regmap must have an identifier.
+// A regmap must manage storage for each register.
+// The recommended way to use a regmap is to define individual registers
+// within the scope of the regmap and instantiate them in the ragmap.
+class UHD_API soft_regmap_t : public soft_regmap_accessor_t, public boost::noncopyable {
+public:
+    soft_regmap_t(const std::string& name) : _name(name) {}
+    virtual ~soft_regmap_t() {};
+
+    /*!
+     * Get the name of this register map
+     */
+    virtual inline const std::string& get_name() const { return _name; }
+
+    /*!
+     * Initialize all registers in this register map using a bus.
+     * Optionally synchronize the register with hardware.
+     * The order of initialization is the same as the order in
+     * which registers were added to the map.
+     */
+    void initialize(wb_iface& iface, bool sync = false) {
+        boost::lock_guard<boost::mutex> lock(_mutex);
+        BOOST_FOREACH(soft_register_base* reg, _reglist) {
+            reg->initialize(iface, sync);
+        }
+    }
+
+    /*!
+     * Flush all registers to hardware.
+     * The order of writing is the same as the order in
+     * which registers were added to the map.
+     */
+    void flush() {
+        boost::lock_guard<boost::mutex> lock(_mutex);
+        BOOST_FOREACH(soft_register_base* reg, _reglist) {
+            reg->flush();
+        }
+    }
+
+    /*!
+     * Refresh all register soft-copies from hardware.
+     * The order of reading is the same as the order in
+     * which registers were added to the map.
+     */
+    void refresh() {
+        boost::lock_guard<boost::mutex> lock(_mutex);
+        BOOST_FOREACH(soft_register_base* reg, _reglist) {
+            reg->refresh();
+        }
+    }
+
+    /*!
+     * Lookup a register object by name.
+     * If a register with "name" is not found, runtime_error is thrown
+     */
+    virtual soft_register_base& lookup(const std::string& name) const {
+        regmap_t::const_iterator iter = _regmap.find(name);
+        if (iter != _regmap.end()) {
+            return *(iter->second);
+        } else {
+            throw uhd::runtime_error("register not found in map: " + name);
+        }
+    }
+
+    /*!
+     * Enumerate all the registers in this map.
+     * Return fully qualified paths.
+     */
+    virtual std::vector<std::string> enumerate() const {
+        std::vector<std::string> temp;
+        BOOST_FOREACH(const regmap_t::value_type& reg, _regmap) {
+            temp.push_back(_name + "/" + reg.first);
+        }
+        return temp;
+    }
+
+protected:
+    enum visibility_t {
+        PUBLIC,     //Is accessible through the soft_regmap_accessor_t interface
+        PRIVATE     //Is NOT accessible through the soft_regmap_accessor_t interface
+    };
+
+    /*!
+     * Add a register to this map with an identifier "name" and visibility
+     */
+    inline void add_to_map(soft_register_base& reg, const std::string& name, const visibility_t visible = PRIVATE) {
+        boost::lock_guard<boost::mutex> lock(_mutex);
+        if (visible == PUBLIC) {
+            //Only add to the map if this register is publicly visible
+            if (not _regmap.insert(regmap_t::value_type(name, &reg)).second) {
+                throw uhd::assertion_error("cannot add two registers with the same name to regmap: " + name);
+            }
+        }
+        _reglist.push_back(&reg);
+    }
+
+private:
+    typedef boost::unordered_map<std::string, soft_register_base*> regmap_t;
+    typedef std::list<soft_register_base*>                         reglist_t;
+
+    const std::string   _name;
+    regmap_t            _regmap;    //For lookups
+    reglist_t           _reglist;   //To maintain order
+    boost::mutex        _mutex;
+};
+
+
+// A regmap database is a collection of regmaps or other regmap databases
+// this allows for efficient encapsulation for multiple registers in a hierarchical
+// fashion.
+// A regmap_db *does not* manage storage for regmaps. It is simply a wrapper.
+class UHD_API soft_regmap_db_t : public soft_regmap_accessor_t, public boost::noncopyable {
+public:
+    typedef boost::shared_ptr<soft_regmap_db_t> sptr;
+
+    /*!
+     * Use the default constructor if this is the top-level DB
+     */
+    soft_regmap_db_t() : _name("") {}
+
+    /*!
+     * Use this constructor if this is a nested DB
+     */
+    soft_regmap_db_t(const std::string& name) : _name(name) {}
+
+    /*!
+     * Get the name of this register map
+     */
+    const std::string& get_name() const { return _name; }
+
+    /*!
+     * Add a regmap to this map with an identifier "name" and visibility
+     */
+    void add(soft_regmap_t& regmap) {
+        boost::lock_guard<boost::mutex> lock(_mutex);
+        _regmaps.push_back(&regmap);
+    }
+
+    /*!
+     * Add a level of regmap_db to this map with an identifier "name" and visibility
+     */
+    void add(soft_regmap_db_t& db) {
+        boost::lock_guard<boost::mutex> lock(_mutex);
+        if (&db == this) {
+            throw uhd::assertion_error("cannot add regmap db to itself" + _name);
+        } else {
+            _regmap_dbs.push_back(&db);
+        }
+    }
+
+    /*!
+     * Lookup a register by path.
+     * A path is defined as a string of "/" separated tokens that scope a register.
+     * The leaf (last token) is the name of the register
+     * The token immediately before the leaf is the name of the register map
+     * If a nested regmap_db is used, the token before the regmap is the db name.
+     * For every nested db, the path has an additional token.
+     * For example:
+     *   radio0/spi_regmap/spi_control_reg
+     */
+    soft_register_base& lookup(const std::string& path) const
+    {
+        //Turn the slash separated path string into tokens
+        std::list<std::string> tokens;
+        BOOST_FOREACH(
+            const std::string& node,
+            boost::tokenizer< boost::char_separator<char> >(path, boost::char_separator<char>("/")))
+        {
+            tokens.push_back(node);
+        }
+        if ((tokens.size() > 2 && tokens.front() == _name) ||   //If this is a nested DB
+            (tokens.size() > 1 && _name == "")) {               //If this is a top-level DB
+            if (_name != "") tokens.pop_front();
+            if (tokens.size() == 2) {                   //2 tokens => regmap/register path
+                BOOST_FOREACH(const soft_regmap_accessor_t* regmap, _regmaps) {
+                    if (regmap->get_name() == tokens.front()) {
+                        return regmap->lookup(tokens.back());
+                    }
+                }
+                throw uhd::runtime_error("could not find register map: " + path);
+            } else if (not _regmap_dbs.empty()) {       //>2 tokens => <1 or more dbs>/regmap/register
+                //Reconstruct path from tokens
+                std::string newpath;
+                BOOST_FOREACH(const std::string& node, tokens) {
+                    newpath += ("/" + node);
+                }
+                //Dispatch path to hierarchical DB
+                BOOST_FOREACH(const soft_regmap_accessor_t* db, _regmap_dbs) {
+                    try {
+                        return db->lookup(newpath.substr(1));
+                    } catch (std::exception& e) {
+                        continue;
+                    }
+                }
+            }
+        }
+        throw uhd::runtime_error("could not find register: " + path);
+    }
+
+    /*!
+     * Enumerate the paths of all registers that this DB can access
+     */
+    virtual std::vector<std::string> enumerate() const {
+        std::vector<std::string> paths;
+        BOOST_FOREACH(const soft_regmap_accessor_t* regmap, _regmaps) {
+            const std::vector<std::string>& regs = regmap->enumerate();
+            paths.insert(paths.end(), regs.begin(), regs.end());
+        }
+        BOOST_FOREACH(const soft_regmap_accessor_t* db, _regmap_dbs) {
+            const std::vector<std::string>& regs = db->enumerate();
+            paths.insert(paths.end(), regs.begin(), regs.end());
+        }
+        return paths;
+    }
+
+private:
+    typedef std::list<soft_regmap_accessor_t*> db_t;
+
+    const std::string   _name;
+    db_t                _regmaps;
+    db_t                _regmap_dbs;
+    boost::mutex        _mutex;
+};
 
 } //namespace uhd
 
diff --git a/host/lib/usrp/multi_usrp.cpp b/host/lib/usrp/multi_usrp.cpp
index 1866255c9..285799674 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,95 @@ 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);
+
+            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("register has invalid bitwidth");
+            }
+
+        } else {
+            throw uhd::not_implemented_error("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);
+
+            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("register has invalid bitwidth: " + path);
+            }
+        } else {
+            throw uhd::not_implemented_error("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>();
+        }
+    }
+
 private:
     device::sptr _dev;
     property_tree::sptr _tree;