diff options
| -rw-r--r-- | host/lib/usrp/b100/b100_ctrl.cpp | 48 | ||||
| -rw-r--r-- | host/lib/usrp/b100/b100_ctrl.hpp | 7 | ||||
| -rw-r--r-- | host/lib/usrp/b100/b100_iface.cpp | 335 | ||||
| -rw-r--r-- | host/lib/usrp/b100/b100_iface.hpp | 73 | ||||
| -rw-r--r-- | host/lib/usrp/b100/b100_impl.cpp | 4 | ||||
| -rw-r--r-- | host/lib/usrp/b100/b100_impl.hpp | 1 | ||||
| -rw-r--r-- | host/lib/usrp/b100/codec_impl.cpp | 149 | ||||
| -rw-r--r-- | host/lib/usrp/b100/dboard_impl.cpp | 185 | ||||
| -rw-r--r-- | host/lib/usrp/b100/dsp_impl.cpp | 217 | ||||
| -rw-r--r-- | host/lib/usrp/b100/io_impl.cpp | 47 | ||||
| -rw-r--r-- | host/lib/usrp/b100/mboard_impl.cpp | 282 | 
11 files changed, 54 insertions, 1294 deletions
| diff --git a/host/lib/usrp/b100/b100_ctrl.cpp b/host/lib/usrp/b100/b100_ctrl.cpp index 3f6d0d86e..be576cc99 100644 --- a/host/lib/usrp/b100/b100_ctrl.cpp +++ b/host/lib/usrp/b100/b100_ctrl.cpp @@ -15,9 +15,7 @@  // along with this program.  If not, see <http://www.gnu.org/licenses/>.  // -#include "../../transport/super_recv_packet_handler.hpp"  #include "b100_ctrl.hpp" -#include "b100_impl.hpp"  #include <uhd/transport/usb_zero_copy.hpp>  #include <uhd/transport/zero_copy.hpp>  #include <uhd/transport/vrt_if_packet.hpp> @@ -36,9 +34,9 @@ bool b100_ctrl_debug = false;  class b100_ctrl_impl : public b100_ctrl {  public: -    b100_ctrl_impl(uhd::transport::zero_copy_if::sptr ctrl_transport) : +    b100_ctrl_impl(uhd::transport::zero_copy_if::sptr ctrl_transport, const async_cb_type &async_cb):          sync_ctrl_fifo(2), -        async_msg_fifo(100), +        _async_cb(async_cb),          _ctrl_transport(ctrl_transport),          _seq(0)      { @@ -56,7 +54,6 @@ public:      }      bool get_ctrl_data(ctrl_data_t &pkt_data, double timeout); -    bool recv_async_msg(uhd::async_metadata_t &async_metadata, double timeout);      void poke32(wb_addr_type addr, boost::uint32_t data){          boost::mutex::scoped_lock lock(_ctrl_mutex); @@ -95,7 +92,7 @@ private:      //änd hërë wë gö ä-Vïkïng för äsynchronous control packets      void viking_marauder_loop(boost::barrier &);      bounded_buffer<ctrl_data_t> sync_ctrl_fifo; -    bounded_buffer<async_metadata_t> async_msg_fifo; +    async_cb_type _async_cb;      boost::thread_group viking_marauders;      uhd::transport::zero_copy_if::sptr _ctrl_transport; @@ -220,36 +217,8 @@ void b100_ctrl_impl::viking_marauder_loop(boost::barrier &spawn_barrier) {              //push it onto the queue              sync_ctrl_fifo.push_with_wait(pkt.data); -        } else { //it's an async status pkt -            //extract the vrt header packet info -            vrt::if_packet_info_t if_packet_info; -            if_packet_info.num_packet_words32 = rbuf->size()/sizeof(boost::uint32_t); -            const boost::uint32_t *vrt_hdr = rbuf->cast<const boost::uint32_t *>(); -            vrt::if_hdr_unpack_le(vrt_hdr, if_packet_info); - -            if(    if_packet_info.sid == B100_TX_ASYNC_SID -               and if_packet_info.packet_type != vrt::if_packet_info_t::PACKET_TYPE_DATA){ -                //fill in the async metadata -                async_metadata_t metadata; -                metadata.channel = 0; -                metadata.has_time_spec = if_packet_info.has_tsi and if_packet_info.has_tsf; -                metadata.time_spec = time_spec_t( -                    time_t(if_packet_info.tsi), size_t(if_packet_info.tsf), 64e6 //FIXME get from clock_ctrl -                ); -                metadata.event_code = async_metadata_t::event_code_t(sph::get_context_code(vrt_hdr, if_packet_info)); -                async_msg_fifo.push_with_pop_on_full(metadata); -                if (metadata.event_code & -                    ( async_metadata_t::EVENT_CODE_UNDERFLOW -                    | async_metadata_t::EVENT_CODE_UNDERFLOW_IN_PACKET) -                ) UHD_MSG(fastpath) << "U"; -                else if (metadata.event_code & -                    ( async_metadata_t::EVENT_CODE_SEQ_ERROR -                    | async_metadata_t::EVENT_CODE_SEQ_ERROR_IN_BURST) -                ) UHD_MSG(fastpath) << "S"; -                continue; -            } -            UHD_MSG(error) << "Control: unknown async response" << std::endl;          } +        else _async_cb(rbuf); //let the async callback handle it      }  } @@ -258,14 +227,9 @@ bool b100_ctrl_impl::get_ctrl_data(ctrl_data_t &pkt_data, double timeout){      return sync_ctrl_fifo.pop_with_timed_wait(pkt_data, timeout);  } -bool b100_ctrl_impl::recv_async_msg(uhd::async_metadata_t &async_metadata, double timeout) { -    boost::this_thread::disable_interruption di; //disable because the wait can throw -    return async_msg_fifo.pop_with_timed_wait(async_metadata, timeout); -} -  /***********************************************************************   * Public make function for b100_ctrl interface   **********************************************************************/ -b100_ctrl::sptr b100_ctrl::make(uhd::transport::zero_copy_if::sptr ctrl_transport){ -    return sptr(new b100_ctrl_impl(ctrl_transport)); +b100_ctrl::sptr b100_ctrl::make(uhd::transport::zero_copy_if::sptr ctrl_transport, const async_cb_type &async_cb){ +    return sptr(new b100_ctrl_impl(ctrl_transport, async_cb));  } diff --git a/host/lib/usrp/b100/b100_ctrl.hpp b/host/lib/usrp/b100/b100_ctrl.hpp index 85e7530f3..eda194ece 100644 --- a/host/lib/usrp/b100/b100_ctrl.hpp +++ b/host/lib/usrp/b100/b100_ctrl.hpp @@ -27,17 +27,20 @@  #include <boost/utility.hpp>  #include "ctrl_packet.hpp"  #include <boost/thread.hpp> +#include <boost/function.hpp>  class b100_ctrl : boost::noncopyable, public wb_iface{  public:      typedef boost::shared_ptr<b100_ctrl> sptr; +    typedef boost::function<void(uhd::transport::managed_recv_buffer::sptr)> async_cb_type;      /*!       * Make a USRP control object from a data transport       * \param ctrl_transport a USB data transport +     * \param async_cb the callback for async messages       * \return a new b100 control object       */ -    static sptr make(uhd::transport::zero_copy_if::sptr ctrl_transport); +    static sptr make(uhd::transport::zero_copy_if::sptr ctrl_transport, const async_cb_type &async_cb);      /*!       * Write a byte vector to an FPGA register @@ -63,8 +66,6 @@ public:       */      virtual bool get_ctrl_data(ctrl_data_t &pkt_data, double timeout) = 0; -    virtual bool recv_async_msg(uhd::async_metadata_t &async_metadata, double timeout) = 0; -  };  #endif /* INCLUDED_B100_CTRL_HPP */ diff --git a/host/lib/usrp/b100/b100_iface.cpp b/host/lib/usrp/b100/b100_iface.cpp deleted file mode 100644 index f0e241541..000000000 --- a/host/lib/usrp/b100/b100_iface.cpp +++ /dev/null @@ -1,335 +0,0 @@ -// -// Copyright 2011 Ettus Research LLC -// -// This program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the -// GNU General Public License for more details. -// -// 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 "b100_impl.hpp" -#include "usrp_commands.h" -#include <uhd/exception.hpp> -#include <uhd/utils/byteswap.hpp> -#include <uhd/utils/safe_call.hpp> -#include <uhd/utils/msg.hpp> -#include <boost/format.hpp> -#include <iomanip> -#include <iostream> - -//FOR TESTING ONLY -#include "b100_regs.hpp" -#include <boost/thread/thread.hpp> -#include "usrp_i2c_addr.h" - -using namespace uhd; -using namespace uhd::usrp; -using namespace uhd::transport; - -/*********************************************************************** - * Constants - **********************************************************************/ -static const bool iface_debug = true; - -/*********************************************************************** - * I2C + FX2 implementation wrapper - **********************************************************************/ -class b100_i2c_fx2_iface : public i2c_iface{ -public: -    b100_i2c_fx2_iface(uhd::usrp::fx2_ctrl::sptr fx2_ctrl){ -        _fx2_ctrl = fx2_ctrl; -    } - -    void write_i2c(boost::uint8_t addr, const byte_vector_t &bytes) -    { -        UHD_ASSERT_THROW(bytes.size() < max_i2c_data_bytes); - -        unsigned char buff[max_i2c_data_bytes]; -        std::copy(bytes.begin(), bytes.end(), buff); - -        int ret = _fx2_ctrl->usrp_i2c_write(addr & 0xff, -                                             buff, -                                             bytes.size()); - -        if (iface_debug && (ret < 0)) -            uhd::runtime_error("USRP: failed i2c write"); -    } - -    byte_vector_t read_i2c(boost::uint8_t addr, size_t num_bytes) -    { -      UHD_ASSERT_THROW(num_bytes < max_i2c_data_bytes); - -      unsigned char buff[max_i2c_data_bytes]; -      int ret = _fx2_ctrl->usrp_i2c_read(addr & 0xff, -                                            buff, -                                            num_bytes); - -      if (iface_debug && ((ret < 0) || (unsigned)ret < (num_bytes))) -          uhd::runtime_error("USRP: failed i2c read"); - -      byte_vector_t out_bytes; -      for (size_t i = 0; i < num_bytes; i++) -          out_bytes.push_back(buff[i]); - -      return out_bytes; -    } - -private: -    static const size_t max_i2c_data_bytes = 64; -    uhd::usrp::fx2_ctrl::sptr _fx2_ctrl; -}; - -/*********************************************************************** - * USRP-E100 interface implementation - **********************************************************************/ -class b100_iface_impl : public b100_iface{ -public: -    /******************************************************************* -     * Structors -     ******************************************************************/ -    b100_iface_impl(uhd::usrp::fx2_ctrl::sptr fx2_ctrl, -                      b100_ctrl::sptr fpga_ctrl) : -                      _fx2_i2c_iface(fx2_ctrl), -                      _fx2_ctrl(fx2_ctrl), -                      _fpga_ctrl(fpga_ctrl) -    { -        this->check_fw_compat(); -        if (fpga_ctrl.get() != NULL){ -            enable_gpif(1); -            i2c_init(); -            this->check_fpga_compat(); -        } -        mb_eeprom = mboard_eeprom_t(get_fx2_i2c_iface(), mboard_eeprom_t::MAP_B000); -    } - -    void check_fw_compat(void){ -        unsigned char data[4]; //useless data buffer -        const boost::uint16_t fw_compat_num = _fx2_ctrl->usrp_control_read( -            VRQ_FW_COMPAT, 0, 0, data, sizeof(data) -        ); -        if (fw_compat_num != B100_FW_COMPAT_NUM){ -            throw uhd::runtime_error(str(boost::format( -                "Expected firmware compatibility number 0x%x, but got 0x%x:\n" -                "The firmware build is not compatible with the host code build." -            ) % B100_FW_COMPAT_NUM % fw_compat_num)); -        } -    } - -    void check_fpga_compat(void){ -        const boost::uint16_t fpga_compat_num = this->peek16(B100_REG_MISC_COMPAT); -        if (fpga_compat_num != B100_FPGA_COMPAT_NUM){ -            throw uhd::runtime_error(str(boost::format( -                "Expected FPGA compatibility number 0x%x, but got 0x%x:\n" -                "The FPGA build is not compatible with the host code build." -            ) % B100_FPGA_COMPAT_NUM % fpga_compat_num)); -        } -    } - -    ~b100_iface_impl(void) -    { -        /* NOP */ -    } - -    /******************************************************************* -     * Peek and Poke -     ******************************************************************/ - -    void poke(boost::uint32_t addr, const ctrl_data_t &data) { -        boost::mutex::scoped_lock lock(_ctrl_mutex); -        _fpga_ctrl->write(addr, data); -    } - -    ctrl_data_t peek(boost::uint32_t addr, size_t len) { -        boost::mutex::scoped_lock lock(_ctrl_mutex); -        return _fpga_ctrl->read(addr, len); -    } - -    void poke16(boost::uint32_t addr, boost::uint16_t value) -    { -        ctrl_data_t words(1); -        words[0] = value; -        poke(addr, words); -    } - -    void poke32(boost::uint32_t addr, boost::uint32_t value) -    { -        //just a subset of poke() to maintain compatibility -        ctrl_data_t words(2); -        words[0] = value & 0x0000FFFF; -        words[1] = value >> 16; -        poke(addr, words); -    } - -    boost::uint32_t peek32(boost::uint32_t addr) -    { -        ctrl_data_t words = peek(addr, 2); -        return boost::uint32_t((boost::uint32_t(words[1]) << 16) | words[0]); -    } - -    boost::uint16_t peek16(boost::uint32_t addr) -    { -        ctrl_data_t words = peek(addr, 1); -        return boost::uint16_t(words[0]); -    } - -    /******************************************************************* -     * I2C -     ******************************************************************/ -    static const boost::uint32_t i2c_datarate = 400000; -    static const boost::uint32_t wishbone_clk = 64000000; //FIXME should go somewhere else - -    void i2c_init(void) { -        //init I2C FPGA interface. -        poke16(B100_REG_I2C_CTRL, 0x0000); -        //set prescalers to operate at 400kHz: WB_CLK is 64MHz... -        boost::uint16_t prescaler = wishbone_clk / (i2c_datarate*5) - 1; -        poke16(B100_REG_I2C_PRESCALER_LO, prescaler & 0xFF); -        poke16(B100_REG_I2C_PRESCALER_HI, (prescaler >> 8) & 0xFF); -        poke16(B100_REG_I2C_CTRL, I2C_CTRL_EN); //enable I2C core -    } - -    static const size_t max_i2c_data_bytes = 64; - -    void i2c_wait_for_xfer(void) -    { -        while(this->peek16(B100_REG_I2C_CMD_STATUS) & I2C_ST_TIP) -            boost::this_thread::sleep(boost::posix_time::milliseconds(10)); -    } - -    bool wait_chk_ack(void) { -        i2c_wait_for_xfer(); -        return (this->peek16(B100_REG_I2C_CMD_STATUS) & I2C_ST_RXACK) == 0; -    } - -    void write_i2c(boost::uint8_t addr, const byte_vector_t &bytes) -    { -        poke16(B100_REG_I2C_DATA, (addr << 1) | 0); //addr and read bit (0) -        poke16(B100_REG_I2C_CMD_STATUS, I2C_CMD_WR | I2C_CMD_START | (bytes.size() == 0 ? I2C_CMD_STOP : 0)); - -        //wait for previous transfer to complete -        if(!wait_chk_ack()) { -            poke16(B100_REG_I2C_CMD_STATUS, I2C_CMD_STOP); -            return; -        } - -        for(size_t i = 0; i < bytes.size(); i++) { -            poke16(B100_REG_I2C_DATA, bytes[i]); -            poke16(B100_REG_I2C_CMD_STATUS, I2C_CMD_WR | ((i == (bytes.size() - 1)) ? I2C_CMD_STOP : 0)); -            if(!wait_chk_ack()) { -                poke16(B100_REG_I2C_CMD_STATUS, I2C_CMD_STOP); -                return; -            } -        } -    } - -    byte_vector_t read_i2c(boost::uint8_t addr, size_t num_bytes) -    { -        byte_vector_t bytes; -        if(num_bytes == 0) return bytes; - -        while (peek16(B100_REG_I2C_CMD_STATUS) & I2C_ST_BUSY); - -        poke16(B100_REG_I2C_DATA, (addr << 1) | 1); //addr and read bit (1) -        poke16(B100_REG_I2C_CMD_STATUS, I2C_CMD_WR | I2C_CMD_START); -        //wait for previous transfer to complete -        if(!wait_chk_ack()) { -            poke16(B100_REG_I2C_CMD_STATUS, I2C_CMD_STOP); -        } - -        for(; num_bytes > 0; num_bytes--) { -            poke16(B100_REG_I2C_CMD_STATUS, I2C_CMD_RD | ((num_bytes == 1) ? (I2C_CMD_STOP | I2C_CMD_NACK) : 0)); -            i2c_wait_for_xfer(); -            boost::uint8_t readback = peek16(B100_REG_I2C_DATA) & 0xFF; -            bytes.push_back(readback); -        } -        return bytes; -    } - -    i2c_iface &get_fx2_i2c_iface(void){ -        return _fx2_i2c_iface; -    } - -    /******************************************************************* -     * SPI interface -     * Eventually this will be replaced with a control-channel system -     * to let the firmware do the actual write/readback cycles. -     * This keeps the bandwidth on the control channel down. -     ******************************************************************/ - -    void spi_wait(void) { -        while(peek32(B100_REG_SPI_CTRL) & SPI_CTRL_GO_BSY); -    } - -    boost::uint32_t transact_spi(int which_slave, -                                 const spi_config_t &config, -                                 boost::uint32_t bits, -                                 size_t num_bits, -                                 bool readback) -    { -        UHD_ASSERT_THROW((num_bits <= 32) && !(num_bits % 8)); - -        int edge_flags = ((config.miso_edge==spi_config_t::EDGE_FALL) ? SPI_CTRL_RXNEG : 0) | -                         ((config.mosi_edge==spi_config_t::EDGE_FALL) ? 0 : SPI_CTRL_TXNEG) -                         ; - -        boost::uint16_t ctrl = SPI_CTRL_ASS | (SPI_CTRL_CHAR_LEN_MASK & num_bits) | edge_flags; - -        spi_wait(); -        poke16(B100_REG_SPI_DIV, 0x0001); // = fpga_clk / 4 -        poke32(B100_REG_SPI_SS, which_slave & 0xFFFF); -        poke32(B100_REG_SPI_TXRX0, bits); -        poke16(B100_REG_SPI_CTRL, ctrl); - -        poke16(B100_REG_SPI_CTRL, ctrl | SPI_CTRL_GO_BSY); -        if(readback) { -            spi_wait(); -            return peek32(B100_REG_SPI_TXRX0); -        } -        else { -            return 0; -        } -    } - -    void reset_gpif(boost::uint16_t ep) { -        _fx2_ctrl->usrp_control_write(VRQ_RESET_GPIF, ep, ep, 0, 0); -    } - -    void enable_gpif(bool en) { -        _fx2_ctrl->usrp_control_write(VRQ_ENABLE_GPIF, en ? 1 : 0, 0, 0, 0); -    } - -    void clear_fpga_fifo(void) { -        _fx2_ctrl->usrp_control_write(VRQ_CLEAR_FPGA_FIFO, 0, 0, 0, 0); -    } - -    void write_uart(boost::uint8_t, const std::string &) { -        throw uhd::not_implemented_error("Unhandled command write_uart()"); -    } -     -    std::string read_uart(boost::uint8_t) { -        throw uhd::not_implemented_error("Unhandled command read_uart()"); -    } - -private: -    b100_i2c_fx2_iface _fx2_i2c_iface; -    uhd::usrp::fx2_ctrl::sptr _fx2_ctrl; -    b100_ctrl::sptr _fpga_ctrl; -    boost::mutex _ctrl_mutex; -}; - -/*********************************************************************** - * Public Make Function - **********************************************************************/ -b100_iface::sptr b100_iface::make(uhd::usrp::fx2_ctrl::sptr fx2_ctrl, -                                      b100_ctrl::sptr fpga_ctrl) -{ -    return b100_iface::sptr(new b100_iface_impl(fx2_ctrl, fpga_ctrl)); -} diff --git a/host/lib/usrp/b100/b100_iface.hpp b/host/lib/usrp/b100/b100_iface.hpp deleted file mode 100644 index f51d91c63..000000000 --- a/host/lib/usrp/b100/b100_iface.hpp +++ /dev/null @@ -1,73 +0,0 @@ -// -// Copyright 2011 Ettus Research LLC -// -// This program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the -// GNU General Public License for more details. -// -// 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_B100_IFACE_HPP -#define INCLUDED_B100_IFACE_HPP - -#include <uhd/usrp/mboard_iface.hpp> -#include <uhd/usrp/mboard_eeprom.hpp> -#include <boost/shared_ptr.hpp> -#include <boost/utility.hpp> -#include <uhd/transport/usb_zero_copy.hpp> -#include "../fx2/fx2_ctrl.hpp" -#include "b100_ctrl.hpp" - -/*! - * The usrp1 interface class: - * Provides a set of functions to implementation layer. - * Including spi, peek, poke, control... - */ -class b100_iface : boost::noncopyable, public uhd::usrp::mboard_iface{ -public: -    typedef boost::shared_ptr<b100_iface> sptr; - -    /*! -     * Make a new b100 interface with the control transport. -     * \param fx2_ctrl the usrp control object -     * \param fpga_ctrl the FPGA interface control object -     * \return a new usrp1 interface object -     */ -    static sptr make(uhd::usrp::fx2_ctrl::sptr fx2_ctrl, -                     b100_ctrl::sptr fpga_ctrl = b100_ctrl::sptr() -    ); - -    /*! -     * Reset the GPIF interface on the FX2 -     * \param which endpoint to reset -     * \return -     */ -    virtual void reset_gpif(boost::uint16_t ep) = 0; - -    /*! -     * Clear the GPIF FIFOs on the FPGA -     * \return -     */ -    virtual void clear_fpga_fifo(void) = 0; - -    /*! -     * Enable/disable the GPIF interfaces on the FX2 -     * \return -     */ -    virtual void enable_gpif(bool en) = 0; - -    //! Get access to the FX2 I2C interface -    virtual uhd::i2c_iface &get_fx2_i2c_iface(void) = 0; - -    uhd::usrp::mboard_eeprom_t mb_eeprom; -}; - -#endif /* INCLUDED_USRP1_IFACE_HPP */ diff --git a/host/lib/usrp/b100/b100_impl.cpp b/host/lib/usrp/b100/b100_impl.cpp index b3bf7b7f0..14ccbbf9b 100644 --- a/host/lib/usrp/b100/b100_impl.cpp +++ b/host/lib/usrp/b100/b100_impl.cpp @@ -201,7 +201,7 @@ b100_impl::b100_impl(const device_addr_t &device_addr){      ////////////////////////////////////////////////////////////////////      // Create controller objects      //////////////////////////////////////////////////////////////////// -    _fpga_ctrl = b100_ctrl::make(_ctrl_transport); +    _fpga_ctrl = b100_ctrl::make(_ctrl_transport, boost::bind(&b100_impl::handle_async_message, this, _1));      this->check_fpga_compat(); //check after making control      _fpga_i2c_ctrl = i2c_core_100::make(_fpga_ctrl, B100_REG_SLAVE(3));      _fpga_spi_ctrl = spi_core_100::make(_fpga_ctrl, B100_REG_SLAVE(2)); @@ -467,5 +467,5 @@ void b100_impl::prepare_gpif(void){      //TODO check the order of this:      enable_gpif(_fx2_ctrl, true);      reset_gpif(_fx2_ctrl, 6); -    clear_fpga_fifo(_fx2_ctrl); +    //clear_fpga_fifo(_fx2_ctrl);  } diff --git a/host/lib/usrp/b100/b100_impl.hpp b/host/lib/usrp/b100/b100_impl.hpp index e0b7aa670..4ae8d2fc1 100644 --- a/host/lib/usrp/b100/b100_impl.hpp +++ b/host/lib/usrp/b100/b100_impl.hpp @@ -125,6 +125,7 @@ private:      void update_tx_subdev_spec(const uhd::usrp::subdev_spec_t &);      void update_ref_source(const std::string &);      void prepare_gpif(void); +    void handle_async_message(uhd::transport::managed_recv_buffer::sptr);  };  #endif /* INCLUDED_b100_IMPL_HPP */ diff --git a/host/lib/usrp/b100/codec_impl.cpp b/host/lib/usrp/b100/codec_impl.cpp deleted file mode 100644 index a959c9d60..000000000 --- a/host/lib/usrp/b100/codec_impl.cpp +++ /dev/null @@ -1,149 +0,0 @@ -// -// Copyright 2011 Ettus Research LLC -// -// This program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the -// GNU General Public License for more details. -// -// 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 "b100_impl.hpp" -#include <uhd/exception.hpp> -#include <uhd/usrp/codec_props.hpp> -#include <boost/bind.hpp> - -using namespace uhd; -using namespace uhd::usrp; - -/*********************************************************************** - * Helper Methods - **********************************************************************/ -void b100_impl::codec_init(void){ -    //make proxies -    _rx_codec_proxy = wax_obj_proxy::make( -        boost::bind(&b100_impl::rx_codec_get, this, _1, _2), -        boost::bind(&b100_impl::rx_codec_set, this, _1, _2) -    ); -    _tx_codec_proxy = wax_obj_proxy::make( -        boost::bind(&b100_impl::tx_codec_get, this, _1, _2), -        boost::bind(&b100_impl::tx_codec_set, this, _1, _2) -    ); -} - -/*********************************************************************** - * RX Codec Properties - **********************************************************************/ -static const std::string ad9862_pga_gain_name = "ad9862 pga"; - -void b100_impl::rx_codec_get(const wax::obj &key_, wax::obj &val){ -    named_prop_t key = named_prop_t::extract(key_); - -    //handle the get request conditioned on the key -    switch(key.as<codec_prop_t>()){ -    case CODEC_PROP_NAME: -        val = std::string("b100 adc - ad9522"); -        return; - -    case CODEC_PROP_OTHERS: -        val = prop_names_t(); -        return; - -    case CODEC_PROP_GAIN_NAMES: -        val = prop_names_t(1, ad9862_pga_gain_name); -        return; - -    case CODEC_PROP_GAIN_RANGE: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        val = b100_codec_ctrl::rx_pga_gain_range; -        return; - -    case CODEC_PROP_GAIN_I: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        val = _codec_ctrl->get_rx_pga_gain('A'); -        return; - -    case CODEC_PROP_GAIN_Q: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        val = _codec_ctrl->get_rx_pga_gain('B'); -        return; - -    default: UHD_THROW_PROP_GET_ERROR(); -    } -} - -void b100_impl::rx_codec_set(const wax::obj &key_, const wax::obj &val){ -    named_prop_t key = named_prop_t::extract(key_); - -    //handle the set request conditioned on the key -    switch(key.as<codec_prop_t>()){ -    case CODEC_PROP_GAIN_I: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        _codec_ctrl->set_rx_pga_gain(val.as<double>(), 'A'); -        return; - -    case CODEC_PROP_GAIN_Q: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        _codec_ctrl->set_rx_pga_gain(val.as<double>(), 'B'); -        return; - -    default: UHD_THROW_PROP_SET_ERROR(); -    } -} - -/*********************************************************************** - * TX Codec Properties - **********************************************************************/ -void b100_impl::tx_codec_get(const wax::obj &key_, wax::obj &val){ -    named_prop_t key = named_prop_t::extract(key_); - -    //handle the get request conditioned on the key -    switch(key.as<codec_prop_t>()){ -    case CODEC_PROP_NAME: -        val = std::string("b100 dac - ad9522"); -        return; - -    case CODEC_PROP_OTHERS: -        val = prop_names_t(); -        return; - -    case CODEC_PROP_GAIN_NAMES: -        val = prop_names_t(1, ad9862_pga_gain_name); -        return; - -    case CODEC_PROP_GAIN_RANGE: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        val = b100_codec_ctrl::tx_pga_gain_range; -        return; - -    case CODEC_PROP_GAIN_I: //only one gain for I and Q -    case CODEC_PROP_GAIN_Q: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        val = _codec_ctrl->get_tx_pga_gain(); -        return; - -    default: UHD_THROW_PROP_GET_ERROR(); -    } -} - -void b100_impl::tx_codec_set(const wax::obj &key_, const wax::obj &val){ -    named_prop_t key = named_prop_t::extract(key_); - -    //handle the set request conditioned on the key -    switch(key.as<codec_prop_t>()){ -    case CODEC_PROP_GAIN_I: //only one gain for I and Q -    case CODEC_PROP_GAIN_Q: -        UHD_ASSERT_THROW(key.name == ad9862_pga_gain_name); -        _codec_ctrl->set_tx_pga_gain(val.as<double>()); -        return; - -    default: UHD_THROW_PROP_SET_ERROR(); -    } -} diff --git a/host/lib/usrp/b100/dboard_impl.cpp b/host/lib/usrp/b100/dboard_impl.cpp deleted file mode 100644 index ed1d4bb1d..000000000 --- a/host/lib/usrp/b100/dboard_impl.cpp +++ /dev/null @@ -1,185 +0,0 @@ -// -// Copyright 2011 Ettus Research LLC -// -// This program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the -// GNU General Public License for more details. -// -// 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 "b100_impl.hpp" -#include "b100_regs.hpp" -#include <uhd/exception.hpp> -#include <uhd/usrp/dboard_props.hpp> -#include <uhd/usrp/subdev_props.hpp> -#include <uhd/usrp/misc_utils.hpp> -#include <boost/bind.hpp> -#include "usrp_i2c_addr.h" - -using namespace uhd; -using namespace uhd::usrp; - -/*********************************************************************** - * Dboard Initialization - **********************************************************************/ -void b100_impl::dboard_init(void){ -    //read the tx and rx dboard eeproms -    _rx_db_eeprom.load(*_iface, I2C_ADDR_RX_A); -    _tx_db_eeprom.load(*_iface, I2C_ADDR_TX_A); -    _gdb_eeprom.load(*_iface, I2C_ADDR_TX_A ^ 5); - -    //create a new dboard interface and manager -    _dboard_iface = make_b100_dboard_iface( -        _iface, _clock_ctrl, _codec_ctrl -    ); -    _dboard_manager = dboard_manager::make( -        _rx_db_eeprom.id, -        ((_gdb_eeprom.id == dboard_id_t::none())? _tx_db_eeprom : _gdb_eeprom).id, -        _dboard_iface -    ); - -    //setup the dboard proxies -    _rx_dboard_proxy = wax_obj_proxy::make( -        boost::bind(&b100_impl::rx_dboard_get, this, _1, _2), -        boost::bind(&b100_impl::rx_dboard_set, this, _1, _2) -    ); -    _tx_dboard_proxy = wax_obj_proxy::make( -        boost::bind(&b100_impl::tx_dboard_get, this, _1, _2), -        boost::bind(&b100_impl::tx_dboard_set, this, _1, _2) -    ); -} - -/*********************************************************************** - * RX Dboard Get - **********************************************************************/ -void b100_impl::rx_dboard_get(const wax::obj &key_, wax::obj &val){ -    named_prop_t key = named_prop_t::extract(key_); - -    //handle the get request conditioned on the key -    switch(key.as<dboard_prop_t>()){ -    case DBOARD_PROP_NAME: -        val = std::string("b100 dboard (rx unit)"); -        return; - -    case DBOARD_PROP_SUBDEV: -        val = _dboard_manager->get_rx_subdev(key.name); -        return; - -    case DBOARD_PROP_SUBDEV_NAMES: -        val = _dboard_manager->get_rx_subdev_names(); -        return; - -    case DBOARD_PROP_DBOARD_EEPROM: -        val = _rx_db_eeprom; -        return; - -    case DBOARD_PROP_DBOARD_IFACE: -        val = _dboard_iface; -        return; - -    case DBOARD_PROP_CODEC: -        val = _rx_codec_proxy->get_link(); -        return; - -    case DBOARD_PROP_GAIN_GROUP: -        val = make_gain_group( -            _rx_db_eeprom.id, -            _dboard_manager->get_rx_subdev(key.name), -            _rx_codec_proxy->get_link(), -            GAIN_GROUP_POLICY_RX -        ); -        return; - -    default: UHD_THROW_PROP_GET_ERROR(); -    } -} - -/*********************************************************************** - * RX Dboard Set - **********************************************************************/ -void b100_impl::rx_dboard_set(const wax::obj &key, const wax::obj &val){ -    switch(key.as<dboard_prop_t>()){ -    case DBOARD_PROP_DBOARD_EEPROM: -        _rx_db_eeprom = val.as<dboard_eeprom_t>(); -        _rx_db_eeprom.store(*_iface, I2C_ADDR_RX_A); -        return; - -    default: UHD_THROW_PROP_SET_ERROR(); -    } -} - -/*********************************************************************** - * TX Dboard Get - **********************************************************************/ -void b100_impl::tx_dboard_get(const wax::obj &key_, wax::obj &val){ -    named_prop_t key = named_prop_t::extract(key_); - -    //handle the get request conditioned on the key -    switch(key.as<dboard_prop_t>()){ -    case DBOARD_PROP_NAME: -        val = std::string("b100 dboard (tx unit)"); -        return; - -    case DBOARD_PROP_SUBDEV: -        val = _dboard_manager->get_tx_subdev(key.name); -        return; - -    case DBOARD_PROP_SUBDEV_NAMES: -        val = _dboard_manager->get_tx_subdev_names(); -        return; - -    case DBOARD_PROP_DBOARD_EEPROM: -        val = _tx_db_eeprom; -        return; - -    case DBOARD_PROP_GBOARD_EEPROM: -        val = _gdb_eeprom; -        return; - -    case DBOARD_PROP_DBOARD_IFACE: -        val = _dboard_iface; -        return; - -    case DBOARD_PROP_CODEC: -        val = _tx_codec_proxy->get_link(); -        return; - -    case DBOARD_PROP_GAIN_GROUP: -        val = make_gain_group( -            _tx_db_eeprom.id, -            _dboard_manager->get_tx_subdev(key.name), -            _tx_codec_proxy->get_link(), -            GAIN_GROUP_POLICY_TX -        ); -        return; - -    default: UHD_THROW_PROP_GET_ERROR(); -    } -} - -/*********************************************************************** - * TX Dboard Set - **********************************************************************/ -void b100_impl::tx_dboard_set(const wax::obj &key, const wax::obj &val){ -    switch(key.as<dboard_prop_t>()){ -    case DBOARD_PROP_DBOARD_EEPROM: -        _tx_db_eeprom = val.as<dboard_eeprom_t>(); -        _tx_db_eeprom.store(*_iface, I2C_ADDR_TX_A); -        return; - -    case DBOARD_PROP_GBOARD_EEPROM: -        _gdb_eeprom = val.as<dboard_eeprom_t>(); -        _gdb_eeprom.store(*_iface, I2C_ADDR_TX_A ^ 5); -        return; - -    default: UHD_THROW_PROP_SET_ERROR(); -    } -} diff --git a/host/lib/usrp/b100/dsp_impl.cpp b/host/lib/usrp/b100/dsp_impl.cpp deleted file mode 100644 index e27894c1a..000000000 --- a/host/lib/usrp/b100/dsp_impl.cpp +++ /dev/null @@ -1,217 +0,0 @@ -// -// Copyright 2011 Ettus Research LLC -// -// This program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the -// GNU General Public License for more details. -// -// 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 "b100_impl.hpp" -#include "b100_regs.hpp" -#include <uhd/usrp/dsp_utils.hpp> -#include <uhd/usrp/dsp_props.hpp> -#include <boost/math/special_functions/round.hpp> -#include <boost/bind.hpp> - -using namespace uhd; -using namespace uhd::usrp; - -/*********************************************************************** - * DSP impl and methods - **********************************************************************/ -struct b100_impl::dsp_impl{ -    uhd::dict<size_t, size_t> ddc_decim; -    uhd::dict<size_t, double> ddc_freq; -    uhd::dict<size_t, size_t> duc_interp; -    uhd::dict<size_t, double> duc_freq; -}; - -/*********************************************************************** - * RX DDC Initialization - **********************************************************************/ -void b100_impl::dsp_init(void){ -    //create new dsp impl -    _dsp_impl = UHD_PIMPL_MAKE(dsp_impl, ()); - -    //bind and initialize the rx dsps -    for (size_t i = 0; i < B100_NUM_RX_DSPS; i++){ -        _rx_dsp_proxies[str(boost::format("DSP%d")%i)] = wax_obj_proxy::make( -            boost::bind(&b100_impl::ddc_get, this, _1, _2, i), -            boost::bind(&b100_impl::ddc_set, this, _1, _2, i) -        ); - -        //initial config and update -        ddc_set(DSP_PROP_FREQ_SHIFT, double(0), i); -        ddc_set(DSP_PROP_HOST_RATE, double(_clock_ctrl->get_fpga_clock_rate()/16), i); - -        //setup the rx control registers -        _iface->poke32(B100_REG_RX_CTRL_CLEAR(i), 1); //reset -        _iface->poke32(B100_REG_RX_CTRL_NSAMPS_PP(i), this->get_max_recv_samps_per_packet()); -        _iface->poke32(B100_REG_RX_CTRL_NCHANNELS(i), 1); -        _iface->poke32(B100_REG_RX_CTRL_VRT_HDR(i), 0 -            | (0x1 << 28) //if data with stream id -            | (0x1 << 26) //has trailer -            | (0x3 << 22) //integer time other -            | (0x1 << 20) //fractional time sample count -        ); -        _iface->poke32(B100_REG_RX_CTRL_VRT_SID(i), B100_DSP_SID_BASE + i); -        _iface->poke32(B100_REG_RX_CTRL_VRT_TLR(i), 0); -        _iface->poke32(B100_REG_TIME64_TPS, size_t(_clock_ctrl->get_fpga_clock_rate())); -    } - -    //bind and initialize the tx dsps -    for (size_t i = 0; i < B100_NUM_TX_DSPS; i++){ -        _tx_dsp_proxies[str(boost::format("DSP%d")%i)] = wax_obj_proxy::make( -            boost::bind(&b100_impl::duc_get, this, _1, _2, i), -            boost::bind(&b100_impl::duc_set, this, _1, _2, i) -        ); - -        //initial config and update -        duc_set(DSP_PROP_FREQ_SHIFT, double(0), i); -        duc_set(DSP_PROP_HOST_RATE, double(_clock_ctrl->get_fpga_clock_rate()/16), i); - -        //init the tx control registers -        _iface->poke32(B100_REG_TX_CTRL_CLEAR_STATE, 1); //reset -        _iface->poke32(B100_REG_TX_CTRL_NUM_CHAN, 0);    //1 channel -        _iface->poke32(B100_REG_TX_CTRL_REPORT_SID, B100_ASYNC_SID); -        _iface->poke32(B100_REG_TX_CTRL_POLICY, B100_FLAG_TX_CTRL_POLICY_NEXT_PACKET); -    } -} - -/*********************************************************************** - * RX DDC Get - **********************************************************************/ -void b100_impl::ddc_get(const wax::obj &key_, wax::obj &val, size_t which_dsp){ -    named_prop_t key = named_prop_t::extract(key_); - -    switch(key.as<dsp_prop_t>()){ -    case DSP_PROP_NAME: -        val = str(boost::format("%s ddc%d") % _iface->get_cname() % which_dsp); -        return; - -    case DSP_PROP_OTHERS: -        val = prop_names_t(); //empty -        return; - -    case DSP_PROP_FREQ_SHIFT: -        val = _dsp_impl->ddc_freq[which_dsp]; -        return; - -    case DSP_PROP_CODEC_RATE: -        val = _clock_ctrl->get_fpga_clock_rate(); -        return; - -    case DSP_PROP_HOST_RATE: -        val = _clock_ctrl->get_fpga_clock_rate()/_dsp_impl->ddc_decim[which_dsp]; -        return; - -    default: UHD_THROW_PROP_GET_ERROR(); -    } -} - -/*********************************************************************** - * RX DDC Set - **********************************************************************/ -void b100_impl::ddc_set(const wax::obj &key_, const wax::obj &val, size_t which_dsp){ -    named_prop_t key = named_prop_t::extract(key_); - -    switch(key.as<dsp_prop_t>()){ - -    case DSP_PROP_STREAM_CMD: -        issue_ddc_stream_cmd(val.as<stream_cmd_t>(), which_dsp); -        return; - -    case DSP_PROP_FREQ_SHIFT:{ -            double new_freq = val.as<double>(); -            _iface->poke32(B100_REG_DSP_RX_FREQ(which_dsp), -                dsp_type1::calc_cordic_word_and_update(new_freq, _clock_ctrl->get_fpga_clock_rate()) -            ); -            _dsp_impl->ddc_freq[which_dsp] = new_freq; //shadow -        } -        return; - -    case DSP_PROP_HOST_RATE:{ -            _dsp_impl->ddc_decim[which_dsp] = boost::math::iround(_clock_ctrl->get_fpga_clock_rate()/val.as<double>()); - -            //set the decimation -            _iface->poke32(B100_REG_DSP_RX_DECIM(which_dsp), dsp_type1::calc_cic_filter_word(_dsp_impl->ddc_decim[which_dsp])); -        } -        this->update_xport_channel_mapping(); //rate changed -> update -        return; - -    default: UHD_THROW_PROP_SET_ERROR(); -    } -} - -/*********************************************************************** - * TX DUC Get - **********************************************************************/ -void b100_impl::duc_get(const wax::obj &key_, wax::obj &val, size_t which_dsp){ -    named_prop_t key = named_prop_t::extract(key_); - -    switch(key.as<dsp_prop_t>()){ -    case DSP_PROP_NAME: -        val = str(boost::format("%s duc%d") % _iface->get_cname() % which_dsp); -        return; - -    case DSP_PROP_OTHERS: -        val = prop_names_t(); //empty -        return; - -    case DSP_PROP_FREQ_SHIFT: -        val = _dsp_impl->duc_freq[which_dsp]; -        return; - -    case DSP_PROP_CODEC_RATE: -        val = _clock_ctrl->get_fpga_clock_rate(); -        return; - -    case DSP_PROP_HOST_RATE: -        val = _clock_ctrl->get_fpga_clock_rate()/_dsp_impl->duc_interp[which_dsp]; -        return; - -    default: UHD_THROW_PROP_GET_ERROR(); -    } -} - -/*********************************************************************** - * TX DUC Set - **********************************************************************/ -void b100_impl::duc_set(const wax::obj &key_, const wax::obj &val, size_t which_dsp){ -    named_prop_t key = named_prop_t::extract(key_); - -    switch(key.as<dsp_prop_t>()){ - -    case DSP_PROP_FREQ_SHIFT:{ -            double new_freq = val.as<double>(); -            _iface->poke32(B100_REG_DSP_TX_FREQ, -                dsp_type1::calc_cordic_word_and_update(new_freq, _clock_ctrl->get_fpga_clock_rate()) -            ); -            _dsp_impl->duc_freq[which_dsp] = new_freq; //shadow -        } -        return; - -    case DSP_PROP_HOST_RATE:{ -            _dsp_impl->duc_interp[which_dsp] = boost::math::iround(_clock_ctrl->get_fpga_clock_rate()/val.as<double>()); - -            //set the interpolation -            _iface->poke32(B100_REG_DSP_TX_INTERP_RATE, dsp_type1::calc_cic_filter_word(_dsp_impl->duc_interp[which_dsp])); - -            //set the scaling -            _iface->poke32(B100_REG_DSP_TX_SCALE_IQ, dsp_type1::calc_iq_scale_word(_dsp_impl->duc_interp[which_dsp])); -        } -        this->update_xport_channel_mapping(); //rate changed -> update -        return; - -    default: UHD_THROW_PROP_SET_ERROR(); -    } -} diff --git a/host/lib/usrp/b100/io_impl.cpp b/host/lib/usrp/b100/io_impl.cpp index 00993ab41..cb8d035a2 100644 --- a/host/lib/usrp/b100/io_impl.cpp +++ b/host/lib/usrp/b100/io_impl.cpp @@ -40,7 +40,7 @@ namespace asio = boost::asio;   **********************************************************************/  struct b100_impl::io_impl{      io_impl(zero_copy_if::sptr data_transport, const size_t recv_width): -        data_transport(data_transport) +        data_transport(data_transport), async_msg_fifo(100/*messages deep*/)      {          for (size_t i = 0; i < recv_width; i++){              typedef bounded_buffer<managed_recv_buffer::sptr> buffs_queue_type; @@ -55,7 +55,7 @@ struct b100_impl::io_impl{      }      zero_copy_if::sptr data_transport; - +    bounded_buffer<async_metadata_t> async_msg_fifo;      std::vector<bounded_buffer<managed_recv_buffer::sptr> *> _buffs_queue;      //gets buffer, determines if its the requested index, @@ -115,6 +115,38 @@ void b100_impl::io_init(void){      _io_impl->send_handler.set_max_samples_per_packet(get_max_send_samps_per_packet());  } +void b100_impl::handle_async_message(managed_recv_buffer::sptr rbuf){ +    vrt::if_packet_info_t if_packet_info; +    if_packet_info.num_packet_words32 = rbuf->size()/sizeof(boost::uint32_t); +    const boost::uint32_t *vrt_hdr = rbuf->cast<const boost::uint32_t *>(); +    try{ +        vrt::if_hdr_unpack_le(vrt_hdr, if_packet_info); +    } +    catch(const std::exception &e){ +        UHD_MSG(error) << "Error (handle_async_message): " << e.what() << std::endl; +    } + +    if (if_packet_info.sid == B100_TX_ASYNC_SID and if_packet_info.packet_type != vrt::if_packet_info_t::PACKET_TYPE_DATA){ +        //fill in the async metadata +        async_metadata_t metadata; +        metadata.channel = 0; +        metadata.has_time_spec = if_packet_info.has_tsi and if_packet_info.has_tsf; +        metadata.time_spec = time_spec_t( +            time_t(if_packet_info.tsi), size_t(if_packet_info.tsf), _clock_ctrl->get_fpga_clock_rate() +        ); +        metadata.event_code = async_metadata_t::event_code_t(sph::get_context_code(vrt_hdr, if_packet_info)); +        _io_impl->async_msg_fifo.push_with_pop_on_full(metadata); +        if (metadata.event_code & +            ( async_metadata_t::EVENT_CODE_UNDERFLOW +            | async_metadata_t::EVENT_CODE_UNDERFLOW_IN_PACKET) +        ) UHD_MSG(fastpath) << "U"; +        else if (metadata.event_code & +            ( async_metadata_t::EVENT_CODE_SEQ_ERROR +            | async_metadata_t::EVENT_CODE_SEQ_ERROR_IN_BURST) +        ) UHD_MSG(fastpath) << "S"; +    } +    else UHD_MSG(error) << "Unknown async packet" << std::endl; +}  void b100_impl::update_tick_rate(const double rate){      boost::mutex::scoped_lock recv_lock = _io_impl->recv_handler.get_scoped_lock(); @@ -177,8 +209,8 @@ void b100_impl::update_tx_subdev_spec(const uhd::usrp::subdev_spec_t &spec){      //bind new callbacks for the handler      for (size_t i = 0; i < _io_impl->send_handler.size(); i++){ -        _io_impl->recv_handler.set_xport_chan_get_buff(i, boost::bind( -            &zero_copy_if::get_recv_buff, _data_transport, _1 +        _io_impl->send_handler.set_xport_chan_get_buff(i, boost::bind( +            &zero_copy_if::get_send_buff, _data_transport, _1          ));      }  } @@ -186,8 +218,11 @@ void b100_impl::update_tx_subdev_spec(const uhd::usrp::subdev_spec_t &spec){  /***********************************************************************   * Async Data   **********************************************************************/ -bool b100_impl::recv_async_msg(uhd::async_metadata_t &md, double timeout){ -    return _fpga_ctrl->recv_async_msg(md, timeout); +bool b100_impl::recv_async_msg( +    async_metadata_t &async_metadata, double timeout +){ +    boost::this_thread::disable_interruption di; //disable because the wait can throw +    return _io_impl->async_msg_fifo.pop_with_timed_wait(async_metadata, timeout);  }  /*********************************************************************** diff --git a/host/lib/usrp/b100/mboard_impl.cpp b/host/lib/usrp/b100/mboard_impl.cpp deleted file mode 100644 index 4f7dc8fce..000000000 --- a/host/lib/usrp/b100/mboard_impl.cpp +++ /dev/null @@ -1,282 +0,0 @@ -// -// Copyright 2011 Ettus Research LLC -// -// This program is free software: you can redistribute it and/or modify -// it under the terms of the GNU General Public License as published by -// the Free Software Foundation, either version 3 of the License, or -// (at your option) any later version. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the -// GNU General Public License for more details. -// -// 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 "b100_impl.hpp" -#include "usrp_commands.h" -#include "fpga_regs_standard.h" -#include "fpga_regs_common.h" -#include "b100_regs.hpp" -#include "usrp_i2c_addr.h" -#include <uhd/usrp/misc_utils.hpp> -#include <uhd/usrp/mboard_props.hpp> -#include <uhd/usrp/dboard_props.hpp> -#include <uhd/usrp/subdev_props.hpp> -#include <uhd/utils/msg.hpp> -#include <uhd/exception.hpp> -#include <uhd/utils/images.hpp> -#include <boost/assign/list_of.hpp> -#include <boost/foreach.hpp> -#include <boost/bind.hpp> -#include <boost/thread/thread.hpp> -#include <uhd/usrp/dsp_utils.hpp> - -using namespace uhd; -using namespace uhd::usrp; - -static const bool b100_mboard_verbose = true; - -/*********************************************************************** - * Mboard Initialization - **********************************************************************/ -void b100_impl::mboard_init(void) -{ -    _mboard_proxy = wax_obj_proxy::make( -                     boost::bind(&b100_impl::mboard_get, this, _1, _2), -                     boost::bind(&b100_impl::mboard_set, this, _1, _2)); - -    //set the ticks per seconds into the vita time control -    _iface->poke32(B100_REG_TIME64_TPS, -        boost::uint32_t(_clock_ctrl->get_fpga_clock_rate()) -    ); -     -    //init the clock config -    _clock_config = clock_config_t::internal(); -    update_clock_config(); -} - -void b100_impl::update_clock_config(void){ -    boost::uint32_t pps_flags = 0; - -    //translate pps polarity enums -    switch(_clock_config.pps_polarity){ -    case clock_config_t::PPS_POS: pps_flags |= B100_FLAG_TIME64_PPS_POSEDGE; break; -    case clock_config_t::PPS_NEG: pps_flags |= B100_FLAG_TIME64_PPS_NEGEDGE; break; -    default: throw uhd::runtime_error("unhandled clock configuration pps polarity"); -    } - -    //set the pps flags -    _iface->poke32(B100_REG_TIME64_FLAGS, pps_flags); - -    //clock source ref 10mhz -    switch(_clock_config.ref_source){ -    case clock_config_t::REF_AUTO: _clock_ctrl->use_auto_ref(); break; -    case clock_config_t::REF_INT: _clock_ctrl->use_internal_ref(); break; -    case clock_config_t::REF_SMA: _clock_ctrl->use_auto_ref(); break; -    default: throw uhd::runtime_error("unhandled clock configuration ref source"); -    } -} - -/*********************************************************************** - * Mboard Get - **********************************************************************/ -void b100_impl::mboard_get(const wax::obj &key_, wax::obj &val){ -    named_prop_t key = named_prop_t::extract(key_); -    static const std::string dboard_name = "A"; - -    //handle the get request conditioned on the key -    switch(key.as<mboard_prop_t>()){ -    case MBOARD_PROP_NAME: -        val = std::string(_iface->get_cname() + " mboard"); -        return; - -    case MBOARD_PROP_OTHERS: -        val = prop_names_t(); -        return; - -    case MBOARD_PROP_RX_DBOARD: -        UHD_ASSERT_THROW(key.name == dboard_name); -        val = _rx_dboard_proxy->get_link(); -        return; - -    case MBOARD_PROP_RX_DBOARD_NAMES: -        val = prop_names_t(1, dboard_name); -        return; - -    case MBOARD_PROP_TX_DBOARD: -        UHD_ASSERT_THROW(key.name == dboard_name); -        val = _tx_dboard_proxy->get_link(); -        return; - -    case MBOARD_PROP_TX_DBOARD_NAMES: -        val = prop_names_t(1, dboard_name); -        return; - -    case MBOARD_PROP_RX_DSP: -        val = _rx_dsp_proxies[key.name]->get_link(); -        return; - -    case MBOARD_PROP_RX_DSP_NAMES: -        val = _rx_dsp_proxies.keys(); -        return; - -    case MBOARD_PROP_TX_DSP: -        val = _tx_dsp_proxies[key.name]->get_link(); -        return; - -    case MBOARD_PROP_TX_DSP_NAMES: -        val = _tx_dsp_proxies.keys(); -        return; - -    case MBOARD_PROP_CLOCK_CONFIG: -        val = _clock_config; -        return; - -    case MBOARD_PROP_RX_SUBDEV_SPEC: -        val = _rx_subdev_spec; -        return; - -    case MBOARD_PROP_TX_SUBDEV_SPEC: -        val = _tx_subdev_spec; -        return; -         -    case MBOARD_PROP_EEPROM_MAP: -        val = _iface->mb_eeprom; -        return; - -    case MBOARD_PROP_TIME_NOW:while(true){ -        uint32_t secs = _iface->peek32(B100_REG_RB_TIME_NOW_SECS); -        uint32_t ticks = _iface->peek32(B100_REG_RB_TIME_NOW_TICKS); -        if (secs != _iface->peek32(B100_REG_RB_TIME_NOW_SECS)) continue; -        val = time_spec_t(secs, ticks, _clock_ctrl->get_fpga_clock_rate()); -        return; -    } -     -    case MBOARD_PROP_TIME_PPS: while(true){ -        uint32_t secs = _iface->peek32(B100_REG_RB_TIME_PPS_SECS); -        uint32_t ticks = _iface->peek32(B100_REG_RB_TIME_PPS_TICKS); -        if (secs != _iface->peek32(B100_REG_RB_TIME_PPS_SECS)) continue; -        val = time_spec_t(secs, ticks, _clock_ctrl->get_fpga_clock_rate()); -        return; -    } - -    case MBOARD_PROP_CLOCK_RATE: -        val = _clock_ctrl->get_fpga_clock_rate(); -        return; - -    default: UHD_THROW_PROP_GET_ERROR(); -    } -} - -/*********************************************************************** - * Mboard Set - **********************************************************************/ -void b100_impl::mboard_set(const wax::obj &key, const wax::obj &val) -{ -    if(key.type() == typeid(std::string)) { -      if(key.as<std::string>() == "load_eeprom") { -        std::string b100_eeprom_image = val.as<std::string>(); -        UHD_MSG(status) << "B100 EEPROM image: " << b100_eeprom_image << std::endl; -        _fx2_ctrl->usrp_load_eeprom(val.as<std::string>()); -      } -      return; -   	} - -    //handle the get request conditioned on the key -    switch(key.as<mboard_prop_t>()){ -    case MBOARD_PROP_TIME_NOW: -    case MBOARD_PROP_TIME_PPS:{ -            time_spec_t time_spec = val.as<time_spec_t>(); -            _iface->poke32(B100_REG_TIME64_TICKS, time_spec.get_tick_count(_clock_ctrl->get_fpga_clock_rate())); -            boost::uint32_t imm_flags = (key.as<mboard_prop_t>() == MBOARD_PROP_TIME_NOW)? 1 : 0; -            _iface->poke32(B100_REG_TIME64_IMM, imm_flags); -            _iface->poke32(B100_REG_TIME64_SECS, boost::uint32_t(time_spec.get_full_secs())); -        } -        return; - -    case MBOARD_PROP_RX_SUBDEV_SPEC:{ -        _rx_subdev_spec = val.as<subdev_spec_t>(); -        verify_rx_subdev_spec(_rx_subdev_spec, _mboard_proxy->get_link()); -        //sanity check -        UHD_ASSERT_THROW(_rx_subdev_spec.size() <= B100_NUM_RX_DSPS); - -        //determine frontend swap IQ from the first channel -        bool fe_swap_iq = false; -        switch(_dboard_manager->get_rx_subdev(_rx_subdev_spec.at(0).sd_name)[SUBDEV_PROP_CONNECTION].as<subdev_conn_t>()){ -        case SUBDEV_CONN_COMPLEX_QI: -        case SUBDEV_CONN_REAL_Q: -            fe_swap_iq = true; -            break; -        default: fe_swap_iq = false; -        } -        _iface->poke32(B100_REG_RX_FE_SWAP_IQ, fe_swap_iq? 1 : 0); - -        //set the dsp mux for each channel -        for (size_t i = 0; i < _rx_subdev_spec.size(); i++){ -            bool iq_swap = false, real_mode = false; -            switch(_dboard_manager->get_rx_subdev(_rx_subdev_spec.at(i).sd_name)[SUBDEV_PROP_CONNECTION].as<subdev_conn_t>()){ -            case SUBDEV_CONN_COMPLEX_IQ: -                iq_swap = fe_swap_iq; -                real_mode = false; -                break; -            case SUBDEV_CONN_COMPLEX_QI: -                iq_swap = not fe_swap_iq; -                real_mode = false; -                break; -            case SUBDEV_CONN_REAL_I: -                iq_swap = fe_swap_iq; -                real_mode = true; -                break; -            case SUBDEV_CONN_REAL_Q: -                iq_swap = not fe_swap_iq; -                real_mode = true; -                break; -            } -            _iface->poke32(B100_REG_DSP_RX_MUX(i), -                (iq_swap?   B100_FLAG_DSP_RX_MUX_SWAP_IQ   : 0) | -                (real_mode? B100_FLAG_DSP_RX_MUX_REAL_MODE : 0) -            ); -        } -        this->update_xport_channel_mapping(); -    }return; - -    case MBOARD_PROP_TX_SUBDEV_SPEC: -        _tx_subdev_spec = val.as<subdev_spec_t>(); -        verify_tx_subdev_spec(_tx_subdev_spec, _mboard_proxy->get_link()); -        //sanity check -        UHD_ASSERT_THROW(_tx_subdev_spec.size() <= B100_NUM_TX_DSPS); -        //set the mux -        _iface->poke32(B100_REG_TX_FE_MUX, dsp_type1::calc_tx_mux_word( -            _dboard_manager->get_tx_subdev(_tx_subdev_spec.front().sd_name)[SUBDEV_PROP_CONNECTION].as<subdev_conn_t>() -        )); -        return; - -    case MBOARD_PROP_EEPROM_MAP: -        // Step1: commit the map, writing only those values set. -        // Step2: readback the entire eeprom map into the iface. -        val.as<mboard_eeprom_t>().commit(*_iface, mboard_eeprom_t::MAP_B000); -        _iface->mb_eeprom = mboard_eeprom_t(*_iface, mboard_eeprom_t::MAP_B000); -        return; - -    case MBOARD_PROP_CLOCK_CONFIG: -        _clock_config = val.as<clock_config_t>(); -        update_clock_config(); -        return; - -    case MBOARD_PROP_CLOCK_RATE: -        UHD_MSG(warning) -            << "You are setting the master clock rate from the API.\n" -            << "You may want to pass this into the device address as master_clock_rate=<rate>.\n" -            << "This way, the clock rate is guaranteed to be initialized first.\n" -            << "See the application notes for USRP-B100 for further instructions.\n" -        ; -        _clock_ctrl->set_fpga_clock_rate(val.as<double>()); -        update_xport_channel_mapping(); -        return; - -    default: UHD_THROW_PROP_SET_ERROR(); -    } -} | 
