diff options
Diffstat (limited to 'host/lib')
| -rw-r--r-- | host/lib/transport/udp_zero_copy_asio.cpp | 12 | ||||
| -rw-r--r-- | host/lib/transport/vrt_packet_handler.hpp | 1 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/CMakeLists.txt | 2 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/clock_ctrl.cpp | 57 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/clock_ctrl.hpp | 12 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/fw_common.h | 8 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/io_impl.cpp | 203 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/mboard_impl.cpp | 69 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/serdes_ctrl.cpp | 46 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/serdes_ctrl.hpp | 40 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/usrp2_impl.cpp | 36 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/usrp2_impl.hpp | 15 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/usrp2_regs.cpp | 2 | ||||
| -rw-r--r-- | host/lib/usrp/usrp2/usrp2_regs.hpp | 10 | 
14 files changed, 341 insertions, 172 deletions
| diff --git a/host/lib/transport/udp_zero_copy_asio.cpp b/host/lib/transport/udp_zero_copy_asio.cpp index c758fa894..12695893e 100644 --- a/host/lib/transport/udp_zero_copy_asio.cpp +++ b/host/lib/transport/udp_zero_copy_asio.cpp @@ -37,14 +37,15 @@ namespace asio = boost::asio;   **********************************************************************/  //Define this to the the boost async io calls to perform receive.  //Otherwise, get_recv_buff uses a blocking receive with timeout. -//#define USE_ASIO_ASYNC_RECV +#define USE_ASIO_ASYNC_RECV  //Define this to the the boost async io calls to perform send.  //Otherwise, the commit callback uses a blocking send.  //#define USE_ASIO_ASYNC_SEND -//enough buffering for half a second of samples at full rate on usrp2 -static const size_t MIN_RECV_SOCK_BUFF_SIZE = size_t(4 * 25e6 * 0.5); +//By default, this buffer is sized insufficiently small. +//For peformance, this buffer should be 10s of megabytes. +static const size_t MIN_RECV_SOCK_BUFF_SIZE = size_t(10e3);  //Large buffers cause more underflow at high rates.  //Perhaps this is due to the kernel scheduling, @@ -321,12 +322,13 @@ private:   **********************************************************************/  template<typename Opt> static void resize_buff_helper(      udp_zero_copy_asio_impl::sptr udp_trans, -    size_t target_size, +    const size_t target_size,      const std::string &name  ){      size_t min_sock_buff_size = 0;      if (name == "recv") min_sock_buff_size = MIN_RECV_SOCK_BUFF_SIZE;      if (name == "send") min_sock_buff_size = MIN_SEND_SOCK_BUFF_SIZE; +    min_sock_buff_size = std::max(min_sock_buff_size, target_size);      std::string help_message;      #if defined(UHD_PLATFORM_LINUX) @@ -347,7 +349,7 @@ template<typename Opt> static void resize_buff_helper(          ) % name % actual_size << std::endl;          if (actual_size < target_size) uhd::warning::post(str(boost::format(              "The %s buffer is smaller than the requested size.\n" -            "The minimum recommended buffer size is %d bytes.\n" +            "The minimum requested buffer size is %d bytes.\n"              "See the transport application notes on buffer resizing.\n%s"          ) % name % min_sock_buff_size % help_message));      } diff --git a/host/lib/transport/vrt_packet_handler.hpp b/host/lib/transport/vrt_packet_handler.hpp index 278bcfeaa..7f8d84308 100644 --- a/host/lib/transport/vrt_packet_handler.hpp +++ b/host/lib/transport/vrt_packet_handler.hpp @@ -91,6 +91,7 @@ template <typename T> UHD_INLINE T get_context_code(          //vrt unpack each managed buffer          uhd::transport::vrt::if_packet_info_t if_packet_info;          for (size_t i = 0; i < state.width; i++){ +            if (state.managed_buffs[i].get() == NULL) continue; //better have a message packet coming up...              //extract packet words and check thats its enough to move on              size_t num_packet_words32 = state.managed_buffs[i]->size()/sizeof(boost::uint32_t); diff --git a/host/lib/usrp/usrp2/CMakeLists.txt b/host/lib/usrp/usrp2/CMakeLists.txt index 527669852..d83c82063 100644 --- a/host/lib/usrp/usrp2/CMakeLists.txt +++ b/host/lib/usrp/usrp2/CMakeLists.txt @@ -38,8 +38,6 @@ IF(ENABLE_USRP2)          ${CMAKE_CURRENT_SOURCE_DIR}/gps_ctrl.cpp          ${CMAKE_CURRENT_SOURCE_DIR}/io_impl.cpp          ${CMAKE_CURRENT_SOURCE_DIR}/mboard_impl.cpp -        ${CMAKE_CURRENT_SOURCE_DIR}/serdes_ctrl.cpp -        ${CMAKE_CURRENT_SOURCE_DIR}/serdes_ctrl.hpp          ${CMAKE_CURRENT_SOURCE_DIR}/usrp2_iface.cpp          ${CMAKE_CURRENT_SOURCE_DIR}/usrp2_iface.hpp          ${CMAKE_CURRENT_SOURCE_DIR}/usrp2_impl.cpp diff --git a/host/lib/usrp/usrp2/clock_ctrl.cpp b/host/lib/usrp/usrp2/clock_ctrl.cpp index 428d5539b..27ccefb2b 100644 --- a/host/lib/usrp/usrp2/clock_ctrl.cpp +++ b/host/lib/usrp/usrp2/clock_ctrl.cpp @@ -22,10 +22,13 @@  #include <uhd/utils/assert.hpp>  #include <boost/cstdint.hpp>  #include <boost/lexical_cast.hpp> +#include <boost/math/special_functions/round.hpp>  #include <iostream>  using namespace uhd; +static const bool enb_test_clk = false; +  /*!   * A usrp2 clock control specific to the ad9510 ic.   */ @@ -66,13 +69,12 @@ public:          this->enable_external_ref(false);          this->enable_rx_dboard_clock(false);          this->enable_tx_dboard_clock(false); +        this->enable_mimo_clock_out(false);          /* private clock enables, must be set here */          this->enable_dac_clock(true);          this->enable_adc_clock(true); - -        /* always driving the mimo reference */ -        this->enable_mimo_clock_out(true); +        this->enable_test_clock(enb_test_clk);      }      ~usrp2_clock_ctrl_impl(void){ @@ -83,6 +85,7 @@ public:          this->enable_dac_clock(false);          this->enable_adc_clock(false);          this->enable_mimo_clock_out(false); +        this->enable_test_clock(false);      }      void enable_mimo_clock_out(bool enb){ @@ -246,6 +249,54 @@ public:      double get_master_clock_rate(void){          return 100e6;      } +     +    void set_mimo_clock_delay(double delay) { +        //delay_val is a 5-bit value (0-31) for fine control +        //the equations below determine delay for a given ramp current, # of caps and fine delay register +        //delay range: +        //range_ns = 200*((caps+3)/i_ramp_ua)*1.3286 +        //offset (zero delay): +        //offset_ns = 0.34 + (1600 - i_ramp_ua)*1e-4 + ((caps-1)/ramp)*6 +        //delay_ns = offset_ns + range_ns * delay / 31 + +        int delay_val = boost::math::iround(delay/9.744e-9*31); + +        if(delay_val == 0) { +            switch(clk_regs.exp) { +            case 5: +                _ad9510_regs.delay_control_out5 = 1; +                break; +            case 6: +                _ad9510_regs.delay_control_out6 = 1; +                break; +            default: +                break; //delay not supported on U2 rev 3 +            } +        } else { +            switch(clk_regs.exp) { +            case 5: +                _ad9510_regs.delay_control_out5 = 0; +                _ad9510_regs.ramp_current_out5 = ad9510_regs_t::RAMP_CURRENT_OUT5_200UA; +                _ad9510_regs.ramp_capacitor_out5 = ad9510_regs_t::RAMP_CAPACITOR_OUT5_4CAPS; +                _ad9510_regs.delay_fine_adjust_out5 = delay_val; +                this->write_reg(0x34); +                this->write_reg(0x35); +                this->write_reg(0x36); +                break; +            case 6: +                _ad9510_regs.delay_control_out6 = 0; +                _ad9510_regs.ramp_current_out6 = ad9510_regs_t::RAMP_CURRENT_OUT6_200UA; +                _ad9510_regs.ramp_capacitor_out6 = ad9510_regs_t::RAMP_CAPACITOR_OUT6_4CAPS; +                _ad9510_regs.delay_fine_adjust_out6 = delay_val; +                this->write_reg(0x38); +                this->write_reg(0x39); +                this->write_reg(0x3A); +                break; +            default: +                break; +            } +        } +    }  private:      /*! diff --git a/host/lib/usrp/usrp2/clock_ctrl.hpp b/host/lib/usrp/usrp2/clock_ctrl.hpp index db6c52c83..9ccbc959e 100644 --- a/host/lib/usrp/usrp2/clock_ctrl.hpp +++ b/host/lib/usrp/usrp2/clock_ctrl.hpp @@ -91,8 +91,18 @@ public:      virtual void enable_test_clock(bool enb) = 0;      /*! -     * TODO other clock control api here.... +     * Enable/disable the ref clock output over the serdes cable. +     * \param enb true to enable +     */ +    virtual void enable_mimo_clock_out(bool enb) = 0; +     +    /*! +     * Set the output delay of the mimo clock +     * Used to synchronise daisy-chained USRPs over the MIMO cable +     * Can also be used to adjust delay for uneven reference cable lengths +     * \param delay the clock delay in seconds       */ +    virtual void set_mimo_clock_delay(double delay) = 0;  }; diff --git a/host/lib/usrp/usrp2/fw_common.h b/host/lib/usrp/usrp2/fw_common.h index f07eaeb21..2cdfdc359 100644 --- a/host/lib/usrp/usrp2/fw_common.h +++ b/host/lib/usrp/usrp2/fw_common.h @@ -33,8 +33,8 @@ extern "C" {  #endif  //fpga and firmware compatibility numbers -#define USRP2_FPGA_COMPAT_NUM 3 -#define USRP2_FW_COMPAT_NUM 7 +#define USRP2_FPGA_COMPAT_NUM 4 +#define USRP2_FW_COMPAT_NUM 8  //used to differentiate control packets over data port  #define USRP2_INVALID_VRT_HEADER 0 @@ -42,7 +42,9 @@ extern "C" {  // udp ports for the usrp2 communication  // Dynamic and/or private ports: 49152-65535  #define USRP2_UDP_CTRL_PORT 49152 -#define USRP2_UDP_DATA_PORT 49153 +//#define USRP2_UDP_UPDATE_PORT 49154 +#define USRP2_UDP_DATA_PORT 49156 +#define USRP2_UDP_ERR0_PORT 49157  ////////////////////////////////////////////////////////////////////////  // I2C addresses diff --git a/host/lib/usrp/usrp2/io_impl.cpp b/host/lib/usrp/usrp2/io_impl.cpp index f8a45f72b..86fb512cc 100644 --- a/host/lib/usrp/usrp2/io_impl.cpp +++ b/host/lib/usrp/usrp2/io_impl.cpp @@ -21,11 +21,13 @@  #include <uhd/utils/byteswap.hpp>  #include <uhd/utils/thread_priority.hpp>  #include <uhd/transport/convert_types.hpp> -#include <uhd/transport/alignment_buffer.hpp> +#include <uhd/transport/bounded_buffer.hpp>  #include <boost/format.hpp>  #include <boost/bind.hpp>  #include <boost/thread.hpp> +#include <boost/date_time/posix_time/posix_time_types.hpp>  #include <iostream> +#include <list>  using namespace uhd;  using namespace uhd::usrp; @@ -108,16 +110,24 @@ private:   * - vrt packet handler states   **********************************************************************/  struct usrp2_impl::io_impl{ -    typedef alignment_buffer<managed_recv_buffer::sptr, time_spec_t> alignment_buffer_type; -    io_impl(size_t num_recv_frames, size_t send_frame_size, size_t width): +    io_impl(size_t send_frame_size, size_t width):          packet_handler_recv_state(width), -        recv_pirate_booty(alignment_buffer_type::make(num_recv_frames-3, width)),          async_msg_fifo(bounded_buffer<async_metadata_t>::make(100/*messages deep*/))      { -        for (size_t i = 0; i < width; i++) fc_mons.push_back( -            flow_control_monitor::sptr(new flow_control_monitor(usrp2_impl::sram_bytes/send_frame_size)) -        ); +        for (size_t i = 0; i < width; i++){ +            fc_mons.push_back(flow_control_monitor::sptr( +                new flow_control_monitor(usrp2_impl::sram_bytes/send_frame_size) +            )); +            //init empty packet infos +            vrt::if_packet_info_t packet_info; +            packet_info.packet_count = 0xf; +            packet_info.has_tsi = true; +            packet_info.tsi = 0; +            packet_info.has_tsf = true; +            packet_info.tsf = 0; +            prev_infos.push_back(packet_info); +        }      }      ~io_impl(void){ @@ -126,11 +136,6 @@ struct usrp2_impl::io_impl{          recv_pirate_crew.join_all();      } -    bool get_recv_buffs(vrt_packet_handler::managed_recv_buffs_t &buffs, double timeout){ -        boost::this_thread::disable_interruption di; //disable because the wait can throw -        return recv_pirate_booty->pop_elems_with_timed_wait(buffs, timeout); -    } -      bool get_send_buffs(          const std::vector<zero_copy_if::sptr> &trans,          vrt_packet_handler::managed_send_buffs_t &buffs, @@ -151,6 +156,15 @@ struct usrp2_impl::io_impl{          return true;      } +    bool get_recv_buffs( +        const std::vector<zero_copy_if::sptr> &xports, +        vrt_packet_handler::managed_recv_buffs_t &buffs, +        double timeout +    ); + +    //previous state for each buffer +    std::vector<vrt::if_packet_info_t> prev_infos; +      //flow control monitors      std::vector<flow_control_monitor::sptr> fc_mons; @@ -162,29 +176,28 @@ struct usrp2_impl::io_impl{      void recv_pirate_loop(zero_copy_if::sptr, usrp2_mboard_impl::sptr, size_t);      boost::thread_group recv_pirate_crew;      bool recv_pirate_crew_raiding; -    alignment_buffer_type::sptr recv_pirate_booty;      bounded_buffer<async_metadata_t>::sptr async_msg_fifo;      boost::mutex spawn_mutex;  };  /***********************************************************************   * Receive Pirate Loop - * - while raiding, loot for recv buffers - * - put booty into the alignment buffer + * - while raiding, loot for message packet + * - update flow control condition count + * - put async message packets into queue   **********************************************************************/  void usrp2_impl::io_impl::recv_pirate_loop( -    zero_copy_if::sptr zc_if, +    zero_copy_if::sptr zc_if_err0,      usrp2_mboard_impl::sptr mboard,      size_t index  ){      set_thread_priority_safe();      recv_pirate_crew_raiding = true; -    size_t next_packet_seq = 0;      spawn_mutex.unlock();      while(recv_pirate_crew_raiding){ -        managed_recv_buffer::sptr buff = zc_if->get_recv_buff(); +        managed_recv_buffer::sptr buff = zc_if_err0->get_recv_buff();          if (not buff.get()) continue; //ignore timeout/error buffers          try{ @@ -194,26 +207,6 @@ void usrp2_impl::io_impl::recv_pirate_loop(              const boost::uint32_t *vrt_hdr = buff->cast<const boost::uint32_t *>();              vrt::if_hdr_unpack_be(vrt_hdr, if_packet_info); -            //handle the rx data stream -            if (if_packet_info.sid == usrp2_impl::RECV_SID){ -                //handle the packet count / sequence number -                if (if_packet_info.packet_count != next_packet_seq){ -                    //std::cerr << "S" << (if_packet_info.packet_count - next_packet_seq)%16; -                    std::cerr << "O" << std::flush; //report overflow (drops in the kernel) -                } -                next_packet_seq = (if_packet_info.packet_count+1)%16; - -                //extract the timespec and round to the nearest packet -                UHD_ASSERT_THROW(if_packet_info.has_tsi and if_packet_info.has_tsf); -                time_spec_t time( -                    time_t(if_packet_info.tsi), size_t(if_packet_info.tsf), mboard->get_master_clock_freq() -                ); - -                //push the packet into the buffer with the new time -                recv_pirate_booty->push_with_pop_on_full(buff, time, index); -                continue; -            } -              //handle a tx async report message              if (if_packet_info.sid == usrp2_impl::ASYNC_SID and if_packet_info.packet_type != vrt::if_packet_info_t::PACKET_TYPE_DATA){ @@ -253,11 +246,10 @@ void usrp2_impl::io_impl::recv_pirate_loop(  void usrp2_impl::io_init(void){      //the assumption is that all data transports should be identical -    const size_t num_recv_frames = _data_transports.front()->get_num_recv_frames();      const size_t send_frame_size = _data_transports.front()->get_send_frame_size();      //create new io impl -    _io_impl = UHD_PIMPL_MAKE(io_impl, (num_recv_frames, send_frame_size, _data_transports.size())); +    _io_impl = UHD_PIMPL_MAKE(io_impl, (send_frame_size, _data_transports.size()));      //create a new pirate thread for each zc if (yarr!!)      for (size_t i = 0; i < _data_transports.size(); i++){ @@ -266,7 +258,7 @@ void usrp2_impl::io_init(void){          //spawn a new pirate to plunder the recv booty          _io_impl->recv_pirate_crew.create_thread(boost::bind(              &usrp2_impl::io_impl::recv_pirate_loop, -            _io_impl.get(), _data_transports.at(i), +            _io_impl.get(), _err0_transports.at(i),              _mboards.at(i), i          ));          //block here until the spawned thread unlocks @@ -318,6 +310,133 @@ size_t usrp2_impl::send(  }  /*********************************************************************** + * Alignment logic on receive + **********************************************************************/ +static UHD_INLINE boost::posix_time::time_duration to_time_dur(double timeout){ +    return boost::posix_time::microseconds(long(timeout*1e6)); +} + +static UHD_INLINE double from_time_dur(const boost::posix_time::time_duration &time_dur){ +    return 1e-6*time_dur.total_microseconds(); +} + +static UHD_INLINE time_spec_t extract_time_spec( +    const vrt::if_packet_info_t &packet_info +){ +    return time_spec_t( //assumes has_tsi and has_tsf are true +        time_t(packet_info.tsi), size_t(packet_info.tsf), +        100e6 //tick rate does not have to be correct for comparison purposes +    ); +} + +static UHD_INLINE void extract_packet_info( +    managed_recv_buffer::sptr &buff, +    vrt::if_packet_info_t &prev_info, +    time_spec_t &time, bool &clear, bool &msg +){ +    //extract packet info +    vrt::if_packet_info_t next_info; +    next_info.num_packet_words32 = buff->size()/sizeof(boost::uint32_t); +    vrt::if_hdr_unpack_be(buff->cast<const boost::uint32_t *>(), next_info); + +    //handle the packet count / sequence number +    if ((prev_info.packet_count+1)%16 != next_info.packet_count){ +        std::cerr << "O" << std::flush; //report overflow (drops in the kernel) +    } + +    time = extract_time_spec(next_info); +    clear = extract_time_spec(prev_info) > time; +    msg = next_info.packet_type != vrt::if_packet_info_t::PACKET_TYPE_DATA; +    prev_info = next_info; +} + +static UHD_INLINE bool handle_msg_packet( +    vrt_packet_handler::managed_recv_buffs_t &buffs, size_t index +){ +    for (size_t i = 0; i < buffs.size(); i++){ +        if (i == index) continue; +        buffs[i].reset(); //set NULL +    } +    return true; +} + +UHD_INLINE bool usrp2_impl::io_impl::get_recv_buffs( +    const std::vector<zero_copy_if::sptr> &xports, +    vrt_packet_handler::managed_recv_buffs_t &buffs, +    double timeout +){ +    if (buffs.size() == 1){ +        buffs[0] = xports[0]->get_recv_buff(timeout); +        if (buffs[0].get() == NULL) return false; +        bool clear, msg; time_spec_t time; //unused variables +        //call extract_packet_info to handle printing the overflows +        extract_packet_info(buffs[0], this->prev_infos[0], time, clear, msg); +        return true; +    } +    //-------------------- begin alignment logic ---------------------// +    boost::system_time exit_time = boost::get_system_time() + to_time_dur(timeout); +    managed_recv_buffer::sptr buff_tmp; +    std::list<size_t> _all_indexes, indexes_to_do; +    for (size_t i = 0; i < buffs.size(); i++) _all_indexes.push_back(i); +    bool clear, msg; +    time_spec_t expected_time; + +    //respond to a clear by starting from scratch +    got_clear: +    indexes_to_do = _all_indexes; +    clear = false; + +    //do an initial pop to load an initial sequence id +    size_t index = indexes_to_do.front(); +    buff_tmp = xports[index]->get_recv_buff(from_time_dur(exit_time - boost::get_system_time())); +    if (buff_tmp.get() == NULL) return false; +    extract_packet_info(buff_tmp, this->prev_infos[index], expected_time, clear, msg); +    if (clear) goto got_clear; +    buffs[index] = buff_tmp; +    if (msg) return handle_msg_packet(buffs, index); +    indexes_to_do.pop_front(); + +    //get an aligned set of elements from the buffers: +    while(indexes_to_do.size() != 0){ + +        //pop an element off for this index +        index = indexes_to_do.front(); +        buff_tmp = xports[index]->get_recv_buff(from_time_dur(exit_time - boost::get_system_time())); +        if (buff_tmp.get() == NULL) return false; +        time_spec_t this_time; +        extract_packet_info(buff_tmp, this->prev_infos[index], this_time, clear, msg); +        if (clear) goto got_clear; +        buffs[index] = buff_tmp; +        if (msg) return handle_msg_packet(buffs, index); + +        //if the sequence id matches: +        //  remove this index from the list and continue +        if (this_time == expected_time){ +            indexes_to_do.pop_front(); +            continue; +        } + +        //if the sequence id is older: +        //  continue with the same index to try again +        else if (this_time < expected_time){ +            continue; +        } + +        //if the sequence id is newer: +        //  use the new expected time for comparison +        //  add all other indexes back into the list +        else{ +            expected_time = this_time; +            indexes_to_do = _all_indexes; +            indexes_to_do.remove(index); +            continue; +        } +    } +    return true; +    //-------------------- end alignment logic -----------------------// +} + +/***********************************************************************   * Receive Data   **********************************************************************/  size_t usrp2_impl::get_max_recv_samps_per_packet(void) const{ @@ -347,7 +466,7 @@ size_t usrp2_impl::recv(          io_type, _rx_otw_type,                     //input and output types to convert          _mboards.front()->get_master_clock_freq(), //master clock tick rate          uhd::transport::vrt::if_hdr_unpack_be, -        boost::bind(&usrp2_impl::io_impl::get_recv_buffs, _io_impl.get(), _1, timeout), +        boost::bind(&usrp2_impl::io_impl::get_recv_buffs, _io_impl.get(), _data_transports, _1, timeout),          boost::bind(&handle_overflow, _mboards, _1)      );  } diff --git a/host/lib/usrp/usrp2/mboard_impl.cpp b/host/lib/usrp/usrp2/mboard_impl.cpp index d97fd1dcd..9ca47cdc3 100644 --- a/host/lib/usrp/usrp2/mboard_impl.cpp +++ b/host/lib/usrp/usrp2/mboard_impl.cpp @@ -27,6 +27,10 @@  #include <iostream>  #include <boost/date_time/posix_time/posix_time.hpp> +static const double mimo_clock_delay_usrp2_rev4 = 4.18e-9; +static const double mimo_clock_delay_usrp_n2xx = 0; //TODO +static const int mimo_clock_sync_delay_cycles = 134; +  using namespace uhd;  using namespace uhd::usrp;  using namespace boost::posix_time; @@ -38,8 +42,9 @@ usrp2_mboard_impl::usrp2_mboard_impl(      size_t index,      transport::udp_simple::sptr ctrl_transport,      transport::zero_copy_if::sptr data_transport, -    size_t recv_samps_per_packet, -    const device_addr_t &flow_control_hints +    transport::zero_copy_if::sptr err0_transport, +    const device_addr_t &device_args, +    size_t recv_samps_per_packet  ):      _index(index),      _iface(usrp2_iface::make(ctrl_transport)) @@ -47,18 +52,21 @@ usrp2_mboard_impl::usrp2_mboard_impl(      //Send a small data packet so the usrp2 knows the udp source port.      //This setup must happen before further initialization occurs      //or the async update packets will cause ICMP destination unreachable. -    transport::managed_send_buffer::sptr send_buff = data_transport->get_send_buff(); +    transport::managed_send_buffer::sptr send_buff;      static const boost::uint32_t data[2] = {          uhd::htonx(boost::uint32_t(0 /* don't care seq num */)),          uhd::htonx(boost::uint32_t(USRP2_INVALID_VRT_HEADER))      }; +    send_buff = data_transport->get_send_buff(); +    std::memcpy(send_buff->cast<void*>(), &data, sizeof(data)); +    send_buff->commit(sizeof(data)); +    send_buff = err0_transport->get_send_buff();      std::memcpy(send_buff->cast<void*>(), &data, sizeof(data));      send_buff->commit(sizeof(data));      //contruct the interfaces to mboard perifs      _clock_ctrl = usrp2_clock_ctrl::make(_iface);      _codec_ctrl = usrp2_codec_ctrl::make(_iface); -    _serdes_ctrl = usrp2_serdes_ctrl::make(_iface);      //_gps_ctrl = usrp2_gps_ctrl::make(_iface);      //if(_gps_ctrl->gps_detected()) std::cout << "GPS time: " << _gps_ctrl->get_time() << std::endl; @@ -98,14 +106,14 @@ usrp2_mboard_impl::usrp2_mboard_impl(      _iface->poke32(_iface->regs.tx_ctrl_policy, U2_FLAG_TX_CTRL_POLICY_NEXT_PACKET);      //setting the cycles per update (disabled by default) -    const double ups_per_sec = flow_control_hints.cast<double>("ups_per_sec", 0.0); +    const double ups_per_sec = device_args.cast<double>("ups_per_sec", 0.0);      if (ups_per_sec > 0.0){          const size_t cycles_per_up = size_t(_clock_ctrl->get_master_clock_rate()/ups_per_sec);          _iface->poke32(_iface->regs.tx_ctrl_cycles_per_up, U2_FLAG_TX_CTRL_UP_ENB | cycles_per_up);      }      //setting the packets per update (enabled by default) -    const double ups_per_fifo = flow_control_hints.cast<double>("ups_per_fifo", 8.0); +    const double ups_per_fifo = device_args.cast<double>("ups_per_fifo", 8.0);      if (ups_per_fifo > 0.0){          const size_t packets_per_up = size_t(usrp2_impl::sram_bytes/ups_per_fifo/data_transport->get_send_frame_size());          _iface->poke32(_iface->regs.tx_ctrl_packets_per_up, U2_FLAG_TX_CTRL_UP_ENB | packets_per_up); @@ -118,6 +126,22 @@ usrp2_mboard_impl::usrp2_mboard_impl(      init_duc_config();      //initialize the clock configuration +    if (device_args.has_key("mimo_mode")){ +        if (device_args["mimo_mode"] == "master"){ +            _mimo_clocking_mode_is_master = true; +        } +        else if (device_args["mimo_mode"] == "slave"){ +            _mimo_clocking_mode_is_master = false; +        } +        else throw std::runtime_error( +            "mimo_mode must be set to master or slave" +        ); +    } +    else { +        _mimo_clocking_mode_is_master = bool(_iface->peek32(_iface->regs.status) & (1 << 8)); +    } +    std::cout << boost::format("mboard%d MIMO %s") % _index % +        (_mimo_clocking_mode_is_master?"master":"slave") << std::endl;      init_clock_config();      //init the codec before the dboard @@ -155,7 +179,6 @@ void usrp2_mboard_impl::update_clock_config(void){      //translate pps source enums      switch(_clock_config.pps_source){      case clock_config_t::PPS_SMA:  pps_flags |= U2_FLAG_TIME64_PPS_SMA;  break; -    case clock_config_t::PPS_MIMO: pps_flags |= U2_FLAG_TIME64_PPS_MIMO; break;      default: throw std::runtime_error("unhandled clock configuration pps source");      } @@ -176,7 +199,6 @@ void usrp2_mboard_impl::update_clock_config(void){          switch(_clock_config.ref_source){          case clock_config_t::REF_INT : _iface->poke32(_iface->regs.misc_ctrl_clock, 0x12); break;          case clock_config_t::REF_SMA : _iface->poke32(_iface->regs.misc_ctrl_clock, 0x1C); break; -        case clock_config_t::REF_MIMO: _iface->poke32(_iface->regs.misc_ctrl_clock, 0x15); break;          default: throw std::runtime_error("unhandled clock configuration reference source");          }          _clock_ctrl->enable_external_ref(true); //USRP2P has an internal 10MHz TCXO @@ -187,7 +209,6 @@ void usrp2_mboard_impl::update_clock_config(void){          switch(_clock_config.ref_source){          case clock_config_t::REF_INT : _iface->poke32(_iface->regs.misc_ctrl_clock, 0x10); break;          case clock_config_t::REF_SMA : _iface->poke32(_iface->regs.misc_ctrl_clock, 0x1C); break; -        case clock_config_t::REF_MIMO: _iface->poke32(_iface->regs.misc_ctrl_clock, 0x15); break;          default: throw std::runtime_error("unhandled clock configuration reference source");          }          _clock_ctrl->enable_external_ref(_clock_config.ref_source != clock_config_t::REF_INT); @@ -195,6 +216,36 @@ void usrp2_mboard_impl::update_clock_config(void){      case usrp2_iface::USRP_NXXX: break;      } + +    //Handle the serdes clocking based on master/slave mode: +    //   - Masters always drive the clock over serdes. +    //   - Slaves always lock to this serdes clock. +    //   - Slaves lock their time over the serdes. +    if (_mimo_clocking_mode_is_master){ +        _clock_ctrl->enable_mimo_clock_out(true); +        switch(_iface->get_rev()){ +        case usrp2_iface::USRP_N200: +        case usrp2_iface::USRP_N210: +            _clock_ctrl->set_mimo_clock_delay(mimo_clock_delay_usrp_n2xx); +            break; + +        case usrp2_iface::USRP2_REV4: +            _clock_ctrl->set_mimo_clock_delay(mimo_clock_delay_usrp2_rev4); +            break; + +        default: break; //not handled +        } +        _iface->poke32(_iface->regs.time64_mimo_sync, 0); +    } +    else{ +        _iface->poke32(_iface->regs.misc_ctrl_clock, 0x15); +        _clock_ctrl->enable_external_ref(true); +        _clock_ctrl->enable_mimo_clock_out(false); +        _iface->poke32(_iface->regs.time64_mimo_sync, +            (1 << 8) | (mimo_clock_sync_delay_cycles & 0xff) +        ); +    } +  }  void usrp2_mboard_impl::set_time_spec(const time_spec_t &time_spec, bool now){ diff --git a/host/lib/usrp/usrp2/serdes_ctrl.cpp b/host/lib/usrp/usrp2/serdes_ctrl.cpp deleted file mode 100644 index 1cda22f45..000000000 --- a/host/lib/usrp/usrp2/serdes_ctrl.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// Copyright 2010 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 "serdes_ctrl.hpp" -#include "usrp2_regs.hpp" - -using namespace uhd; - -/*! - * A usrp2 serdes control implementation - */ -class usrp2_serdes_ctrl_impl : public usrp2_serdes_ctrl{ -public: -    usrp2_serdes_ctrl_impl(usrp2_iface::sptr iface){ -        _iface = iface; -        _iface->poke32(_iface->regs.misc_ctrl_serdes, U2_FLAG_MISC_CTRL_SERDES_ENABLE | U2_FLAG_MISC_CTRL_SERDES_RXEN); -    } - -    ~usrp2_serdes_ctrl_impl(void){ -        _iface->poke32(_iface->regs.misc_ctrl_serdes, 0); //power-down -    } - -private: -    usrp2_iface::sptr _iface; -}; - -/*********************************************************************** - * Public make function for the usrp2 serdes control - **********************************************************************/ -usrp2_serdes_ctrl::sptr usrp2_serdes_ctrl::make(usrp2_iface::sptr iface){ -    return sptr(new usrp2_serdes_ctrl_impl(iface)); -} diff --git a/host/lib/usrp/usrp2/serdes_ctrl.hpp b/host/lib/usrp/usrp2/serdes_ctrl.hpp deleted file mode 100644 index 3c909c531..000000000 --- a/host/lib/usrp/usrp2/serdes_ctrl.hpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// Copyright 2010 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_SERDES_CTRL_HPP -#define INCLUDED_SERDES_CTRL_HPP - -#include "usrp2_iface.hpp" -#include <boost/shared_ptr.hpp> -#include <boost/utility.hpp> - -class usrp2_serdes_ctrl : boost::noncopyable{ -public: -    typedef boost::shared_ptr<usrp2_serdes_ctrl> sptr; - -    /*! -     * Make a serdes control object for the usrp2 serdes port. -     * \param _iface a pointer to the usrp2 interface object -     * \return a new serdes control object -     */ -    static sptr make(usrp2_iface::sptr iface); - -    //TODO fill me in with virtual methods - -}; - -#endif /* INCLUDED_SERDES_CTRL_HPP */ diff --git a/host/lib/usrp/usrp2/usrp2_impl.cpp b/host/lib/usrp/usrp2/usrp2_impl.cpp index c3bbe4d65..f910999d4 100644 --- a/host/lib/usrp/usrp2/usrp2_impl.cpp +++ b/host/lib/usrp/usrp2/usrp2_impl.cpp @@ -197,24 +197,36 @@ static device_addrs_t usrp2_find(const device_addr_t &hint_){   * Make   **********************************************************************/  static device::sptr usrp2_make(const device_addr_t &device_addr){ -sep_indexed_dev_addrs(device_addr); + +    //setup the dsp transport hints (default to a large recv buff) +    device_addr_t dsp_xport_hints = device_addr; +    if (not dsp_xport_hints.has_key("recv_buff_size")){ +        //set to half-a-second of buffering at max rate +        dsp_xport_hints["recv_buff_size"] = "50e6"; +    } +      //create a ctrl and data transport for each address      std::vector<udp_simple::sptr> ctrl_transports;      std::vector<zero_copy_if::sptr> data_transports; +    std::vector<zero_copy_if::sptr> err0_transports; +    const device_addrs_t device_addrs = sep_indexed_dev_addrs(device_addr); -    BOOST_FOREACH(const device_addr_t &dev_addr_i, sep_indexed_dev_addrs(device_addr)){ +    BOOST_FOREACH(const device_addr_t &dev_addr_i, device_addrs){          ctrl_transports.push_back(udp_simple::make_connected(              dev_addr_i["addr"], num2str(USRP2_UDP_CTRL_PORT)          ));          data_transports.push_back(udp_zero_copy::make( -            dev_addr_i["addr"], num2str(USRP2_UDP_DATA_PORT), device_addr +            dev_addr_i["addr"], num2str(USRP2_UDP_DATA_PORT), dsp_xport_hints +        )); +        err0_transports.push_back(udp_zero_copy::make( +            dev_addr_i["addr"], num2str(USRP2_UDP_ERR0_PORT), device_addr_t()          ));      }      //create the usrp2 implementation guts -    return device::sptr( -        new usrp2_impl(ctrl_transports, data_transports, device_addr) -    ); +    return device::sptr(new usrp2_impl( +        ctrl_transports, data_transports, err0_transports, device_addrs +    ));  }  UHD_STATIC_BLOCK(register_usrp2_device){ @@ -227,9 +239,11 @@ UHD_STATIC_BLOCK(register_usrp2_device){  usrp2_impl::usrp2_impl(      std::vector<udp_simple::sptr> ctrl_transports,      std::vector<zero_copy_if::sptr> data_transports, -    const device_addr_t &flow_control_hints +    std::vector<zero_copy_if::sptr> err0_transports, +    const device_addrs_t &device_args  ): -    _data_transports(data_transports) +    _data_transports(data_transports), +    _err0_transports(err0_transports)  {      //setup rx otw type      _rx_otw_type.width = 16; @@ -244,11 +258,11 @@ usrp2_impl::usrp2_impl(      //!!!!! set the otw type here before continuing, its used below      //create a new mboard handler for each control transport -    for(size_t i = 0; i < ctrl_transports.size(); i++){ +    for(size_t i = 0; i < device_args.size(); i++){          _mboards.push_back(usrp2_mboard_impl::sptr(new usrp2_mboard_impl(              i, ctrl_transports[i], data_transports[i], -            this->get_max_recv_samps_per_packet(), -            flow_control_hints +            err0_transports[i], device_args[i], +            this->get_max_recv_samps_per_packet()          )));          //use an empty name when there is only one mboard          std::string name = (ctrl_transports.size() > 1)? boost::lexical_cast<std::string>(i) : ""; diff --git a/host/lib/usrp/usrp2/usrp2_impl.hpp b/host/lib/usrp/usrp2/usrp2_impl.hpp index aa8eb0155..9cd27ee41 100644 --- a/host/lib/usrp/usrp2/usrp2_impl.hpp +++ b/host/lib/usrp/usrp2/usrp2_impl.hpp @@ -22,7 +22,6 @@  #include "clock_ctrl.hpp"  #include "codec_ctrl.hpp"  #include "gps_ctrl.hpp" -#include "serdes_ctrl.hpp"  #include <uhd/device.hpp>  #include <uhd/utils/pimpl.hpp>  #include <uhd/types/dict.hpp> @@ -86,8 +85,9 @@ public:          size_t index,          uhd::transport::udp_simple::sptr,          uhd::transport::zero_copy_if::sptr, -        size_t recv_samps_per_packet, -        const uhd::device_addr_t &flow_control_hints +        uhd::transport::zero_copy_if::sptr, +        const uhd::device_addr_t &device_args, +        size_t recv_samps_per_packet      );      ~usrp2_mboard_impl(void); @@ -100,12 +100,12 @@ public:  private:      size_t _index;      bool _continuous_streaming; +    bool _mimo_clocking_mode_is_master;      //interfaces      usrp2_iface::sptr _iface;      usrp2_clock_ctrl::sptr _clock_ctrl;      usrp2_codec_ctrl::sptr _codec_ctrl; -    usrp2_serdes_ctrl::sptr _serdes_ctrl;      usrp2_gps_ctrl::sptr _gps_ctrl;      //properties for this mboard @@ -187,12 +187,14 @@ public:       * Create a new usrp2 impl base.       * \param ctrl_transports the udp transports for control       * \param data_transports the udp transports for data -     * \param flow_control_hints optional flow control params +     * \param err0_transports the udp transports for error +     * \param device_args optional misc device parameters       */      usrp2_impl(          std::vector<uhd::transport::udp_simple::sptr> ctrl_transports,          std::vector<uhd::transport::zero_copy_if::sptr> data_transports, -        const uhd::device_addr_t &flow_control_hints +        std::vector<uhd::transport::zero_copy_if::sptr> err0_transports, +        const uhd::device_addrs_t &device_args      );      ~usrp2_impl(void); @@ -223,6 +225,7 @@ private:      //io impl methods and members      std::vector<uhd::transport::zero_copy_if::sptr> _data_transports; +    std::vector<uhd::transport::zero_copy_if::sptr> _err0_transports;      uhd::otw_type_t _rx_otw_type, _tx_otw_type;      UHD_PIMPL_DECL(io_impl) _io_impl;      void io_init(void); diff --git a/host/lib/usrp/usrp2/usrp2_regs.cpp b/host/lib/usrp/usrp2/usrp2_regs.cpp index a566dbce7..13f475413 100644 --- a/host/lib/usrp/usrp2/usrp2_regs.cpp +++ b/host/lib/usrp/usrp2/usrp2_regs.cpp @@ -57,6 +57,8 @@ usrp2_regs_t usrp2_get_regs(bool use_n2xx_map) {    x.time64_flags = sr_addr(misc_output_base, x.sr_time64 + 2);    x.time64_imm = sr_addr(misc_output_base, x.sr_time64 + 3);    x.time64_tps = sr_addr(misc_output_base, x.sr_time64 + 4); +  x.time64_mimo_sync = sr_addr(misc_output_base, x.sr_time64 + 5); +  x.status = bp_base + 4*8;    x.time64_secs_rb_imm = bp_base + 4*10;    x.time64_ticks_rb_imm = bp_base + 4*11;    x.compat_num_rb = bp_base + 4*12; diff --git a/host/lib/usrp/usrp2/usrp2_regs.hpp b/host/lib/usrp/usrp2/usrp2_regs.hpp index 7014c192e..68b8796d2 100644 --- a/host/lib/usrp/usrp2/usrp2_regs.hpp +++ b/host/lib/usrp/usrp2/usrp2_regs.hpp @@ -25,10 +25,10 @@  #define USRP2_ATR_BASE          0xE400  #define USRP2_BP_STATUS_BASE    0xCC00 -#define USRP2P_MISC_OUTPUT_BASE 0x2000 -#define USRP2P_GPIO_BASE        0x3200 -#define USRP2P_ATR_BASE         0x3800 -#define USRP2P_BP_STATUS_BASE   0x3300 +#define USRP2P_MISC_OUTPUT_BASE 0x5000 +#define USRP2P_GPIO_BASE        0x6200 +#define USRP2P_ATR_BASE         0x6800 +#define USRP2P_BP_STATUS_BASE   0x6300  typedef struct {      int sr_misc; @@ -57,6 +57,8 @@ typedef struct {      int time64_flags; // flags -- see chart below      int time64_imm; // set immediate (0=latch on next pps, 1=latch immediate, default=0)      int time64_tps; // ticks per second rollover count +    int time64_mimo_sync; +    int status;      int time64_secs_rb_imm;      int time64_ticks_rb_imm;      int time64_secs_rb_pps; | 
