diff options
Diffstat (limited to 'host/lib')
| -rw-r--r-- | host/lib/convert/convert_unpack_sc12.cpp | 61 | ||||
| -rw-r--r-- | host/lib/transport/libusb1_base.cpp | 23 | ||||
| -rw-r--r-- | host/lib/transport/libusb1_zero_copy.cpp | 99 | ||||
| -rw-r--r-- | host/lib/usrp/b100/b100_impl.cpp | 6 | ||||
| -rw-r--r-- | host/lib/usrp/b200/b200_iface.cpp | 42 | ||||
| -rw-r--r-- | host/lib/usrp/b200/b200_iface.hpp | 16 | ||||
| -rw-r--r-- | host/lib/usrp/b200/b200_impl.cpp | 21 | ||||
| -rw-r--r-- | host/lib/usrp/b200/b200_impl.hpp | 8 | ||||
| -rw-r--r-- | host/lib/usrp/b200/b200_io_impl.cpp | 32 | ||||
| -rw-r--r-- | host/lib/usrp/cores/radio_ctrl_core_3000.cpp | 121 | ||||
| -rw-r--r-- | host/lib/usrp/cores/radio_ctrl_core_3000.hpp | 5 | ||||
| -rw-r--r-- | host/lib/usrp/dboard/db_dbsrx2.cpp | 6 | ||||
| -rw-r--r-- | host/lib/usrp/dboard/db_sbx_common.cpp | 132 | ||||
| -rw-r--r-- | host/lib/usrp/dboard/db_sbx_common.hpp | 28 | ||||
| -rw-r--r-- | host/lib/usrp/dboard/db_sbx_version3.cpp | 118 | ||||
| -rw-r--r-- | host/lib/usrp/dboard/db_sbx_version4.cpp | 120 | ||||
| -rw-r--r-- | host/lib/usrp/usrp1/usrp1_impl.cpp | 8 | ||||
| -rw-r--r-- | host/lib/utils/paths.cpp | 22 | ||||
| -rw-r--r-- | host/lib/utils/tasks.cpp | 99 | 
19 files changed, 637 insertions, 330 deletions
| diff --git a/host/lib/convert/convert_unpack_sc12.cpp b/host/lib/convert/convert_unpack_sc12.cpp index 6583eb21f..a2aec2ae5 100644 --- a/host/lib/convert/convert_unpack_sc12.cpp +++ b/host/lib/convert/convert_unpack_sc12.cpp @@ -32,6 +32,17 @@ struct item32_sc12_3x      item32_t line2;  }; +/* + * convert_sc12_item32_3_to_star_4 takes in 3 lines with 32 bit each + * and converts them 4 samples of type 'std::complex<type>'. + * The structure of the 3 lines is as follows: + *  _ _ _ _ _ _ _ _ + * |_ _ _1_ _ _|_ _| + * |_2_ _ _|_ _ _3_| + * |_ _|_ _ _4_ _ _| + * + * The numbers mark the position of one complex sample. + */  template <typename type, tohost32_type tohost>  void convert_sc12_item32_3_to_star_4  ( @@ -84,17 +95,48 @@ struct convert_sc12_item32_1_to_star_1 : public converter          _scalar = scalar/unpack_growth;      } +    /* +     * This converter takes in 24 bits complex samples, 12 bits I and 12 bits Q, and converts them to type 'std::complex<type>'. +     * 'type' is usually 'float'. +     * For the converter to work correctly the used managed_buffer which holds all samples of one packet has to be 32 bits aligned. +     * We assume 32 bits to be one line. This said the converter must be aware where it is supposed to start within 3 lines. +     * +     */      void operator()(const input_type &inputs, const output_type &outputs, const size_t nsamps)      { -        const item32_sc12_3x *input = reinterpret_cast<const item32_sc12_3x *>(size_t(inputs[0]) & ~0x3); +        /* +         * Looking at the line structure above we can identify 4 cases. +         * Each corresponds to the start of a different sample within a 3 line block. +         * head_samps derives the number of samples left within one block. +         * Then the number of bytes the converter has to rewind are calculated. +         */ +        const size_t head_samps = size_t(inputs[0]) & 0x3; +        size_t rewind = 0; +        switch(head_samps) +        { +            case 0: break; +            case 1: rewind = 9; break; +            case 2: rewind = 6; break; +            case 3: rewind = 3; break; +        } + +        /* +         * The pointer *input now points to the head of a 3 line block. +         */ +        const item32_sc12_3x *input = reinterpret_cast<const item32_sc12_3x *>(size_t(inputs[0]) - rewind);          std::complex<type> *output = reinterpret_cast<std::complex<type> *>(outputs[0]);          //helper variables          std::complex<type> dummy0, dummy1, dummy2;          size_t i = 0, o = 0; -        //handle the head case -        const size_t head_samps = size_t(inputs[0]) & 0x3; +        /* +         * handle the head case +         * head_samps holds the number of samples left in a block. +         * The 3 line converter is called for the whole block and already processed samples are dumped. +         * We don't run into the risk of a SIGSEGV because input will always point to valid memory within a managed_buffer. +         * Furthermore the bytes in a buffer remain unchanged after they have been copied into it. +         */          switch (head_samps)          {          case 0: break; //no head @@ -111,7 +153,18 @@ struct convert_sc12_item32_1_to_star_1 : public converter              i++; o += 4;          } -        //handle the tail case +        /* +         * handle the tail case +         * The converter can be called with any number of samples to be converted. +         * This can end up in only a part of a block to be converted in one call. +         * We never have to worry about SIGSEGVs here as long as we end in the middle of a managed_buffer. +         * If we are at the end of managed_buffer there are 2 precautions to prevent SIGSEGVs. +         * Firstly only a read operation is performed. +         * Secondly managed_buffers allocate a fixed size memory which is always larger than the actually used size. +         * e.g. The current sample maximum is 2000 samples in a packet over USB. +         * With sc12 samples a packet consists of 6000kb but managed_buffers allocate 16kb each. +         * Thus we don't run into problems here either. +         */          const size_t tail_samps = nsamps - o;          switch (tail_samps)          { diff --git a/host/lib/transport/libusb1_base.cpp b/host/lib/transport/libusb1_base.cpp index 0ef53db0a..8bd0f4354 100644 --- a/host/lib/transport/libusb1_base.cpp +++ b/host/lib/transport/libusb1_base.cpp @@ -19,10 +19,12 @@  #include <uhd/exception.hpp>  #include <uhd/utils/msg.hpp>  #include <uhd/utils/log.hpp> +#include <uhd/utils/tasks.hpp>  #include <uhd/types/dict.hpp>  #include <boost/weak_ptr.hpp>  #include <boost/thread/mutex.hpp>  #include <boost/foreach.hpp> +#include <boost/bind.hpp>  #include <cstdlib>  #include <iostream> @@ -37,9 +39,11 @@ public:      libusb_session_impl(void){          UHD_ASSERT_THROW(libusb_init(&_context) == 0);          libusb_set_debug(_context, debug_level); +        task_handler = task::make(boost::bind(&libusb_session_impl::libusb_event_handler_task, this, _context));      }      ~libusb_session_impl(void){ +        task_handler.reset();          libusb_exit(_context);      } @@ -49,6 +53,21 @@ public:  private:      libusb_context *_context; +    task::sptr task_handler; + +    /* +     * Task to handle libusb events.  There should only be one thread per libusb_context handling events. +     * Using more than one thread can result in excessive CPU usage in kernel space (presumably from locking/waiting). +     * The libusb documentation says it is safe, which it is, but it neglects to state the cost in CPU usage. +     * Just don't do it! +     */ +    UHD_INLINE void libusb_event_handler_task(libusb_context *context) +    { +        timeval tv; +        tv.tv_sec = 0; +        tv.tv_usec = 100000; +        libusb_handle_events_timeout(context, &tv); +    }  };  libusb::session::sptr libusb::session::get_global_session(void){ @@ -274,6 +293,10 @@ public:          return libusb::device_descriptor::make(this->get_device())->get().idProduct;      } +    bool firmware_loaded() { +        return (get_manufacturer() == "Ettus Research LLC"); +    } +  private:      libusb::device::sptr _dev; //always keep a reference to device  }; diff --git a/host/lib/transport/libusb1_zero_copy.cpp b/host/lib/transport/libusb1_zero_copy.cpp index 197e257da..2d18e1623 100644 --- a/host/lib/transport/libusb1_zero_copy.cpp +++ b/host/lib/transport/libusb1_zero_copy.cpp @@ -73,6 +73,15 @@ struct lut_result_t      int completed;      libusb_transfer_status status;      int actual_length; +    boost::mutex mut; +    boost::condition_variable usb_transfer_complete; +}; + +// Created to be used as an argument to boost::condition_variable::timed_wait() function +struct lut_result_completed { +    const lut_result_t& _result; +    lut_result_completed(const lut_result_t& result):_result(result) {} +    bool operator()() const {return (_result.completed ? true : false);}  };  /*! @@ -84,48 +93,11 @@ struct lut_result_t  static void LIBUSB_CALL libusb_async_cb(libusb_transfer *lut)  {      lut_result_t *r = (lut_result_t *)lut->user_data; -    r->completed = 1; +    boost::lock_guard<boost::mutex> lock(r->mut);      r->status = lut->status;      r->actual_length = lut->actual_length; -} - -/*! - * Wait for a managed buffer to become complete. - * - * This routine processes async events until the transaction completes. - * We must call the libusb handle events in a loop because the handler - * may complete managed buffers other than the one we are waiting on. - * - * We cannot determine if handle events timed out or processed an event. - * Therefore, the timeout condition is handled by using boost system time. - * - * \param ctx the libusb context structure - * \param timeout the wait timeout in seconds - * \param completed a reference to the completed flag - * \return true for completion, false for timeout - */ -UHD_INLINE bool wait_for_completion(libusb_context *ctx, const double timeout, int &completed) -{ -    //already completed by a previous call? -    if (completed) return true; - -    //perform a non-blocking event handle -    timeval tv; -    tv.tv_sec = 0; -    tv.tv_usec = 0; -    libusb_handle_events_timeout_completed(ctx, &tv, &completed); -    if (completed) return true; - -    //finish the rest with a timeout loop -    const boost::system_time timeout_time = boost::get_system_time() + boost::posix_time::microseconds(long(timeout*1000000)); -    while (not completed and (boost::get_system_time() < timeout_time)){ -        timeval tv; -        tv.tv_sec = 0; -        tv.tv_usec = 10000; /*10ms*/ -        libusb_handle_events_timeout_completed(ctx, &tv, &completed); -    } - -    return completed; +    r->completed = 1; +    r->usb_transfer_complete.notify_one();  // wake up thread waiting in wait_for_completion() member function below  }  /*********************************************************************** @@ -154,7 +126,7 @@ public:      template <typename buffer_type>      UHD_INLINE typename buffer_type::sptr get_new(const double timeout)      { -        if (wait_for_completion(_ctx, timeout, result.completed)) +        if (wait_for_completion(timeout))          {              if (result.status != LIBUSB_TRANSFER_COMPLETED) throw uhd::runtime_error(str(boost::format(                  "usb %s transfer status: %d") % _name % int(result.status))); @@ -164,9 +136,31 @@ public:          return typename buffer_type::sptr();      } +    // This is public because it is accessed from the libusb_zero_copy_single constructor      lut_result_t result; +    /*! +     * Wait for a managed buffer to become complete. +     * +     * \param timeout the wait timeout in seconds.  A negative value will wait forever. +     * \return true for completion, false for timeout +     */ +    UHD_INLINE bool wait_for_completion(const double timeout) +    { +        boost::unique_lock<boost::mutex> lock(result.mut); +        if (!result.completed) { +            if (timeout < 0.0) { +                result.usb_transfer_complete.wait(lock); +            } else { +                const boost::system_time timeout_time = boost::get_system_time() + boost::posix_time::microseconds(long(timeout*1000000)); +                result.usb_transfer_complete.timed_wait(lock, timeout_time, lut_result_completed(result)); +            } +        } +        return result.completed; +    } +  private: +      boost::function<void(libusb_zero_copy_mb *)> _release_cb;      const bool _is_recv;      const std::string _name; @@ -252,8 +246,6 @@ public:      ~libusb_zero_copy_single(void)      { -        libusb_context *ctx = libusb::session::get_global_session()->get_context(); -          //cancel all transfers          BOOST_FOREACH(libusb_transfer *lut, _all_luts)          { @@ -261,8 +253,10 @@ public:          }          //process all transfers until timeout occurs -        int completed = 0; -        wait_for_completion(ctx, 0.01, completed); +        BOOST_FOREACH(libusb_zero_copy_mb *mb, _enqueued) +        { +            mb->wait_for_completion(0.01); +        }          //free all transfers          BOOST_FOREACH(libusb_transfer *lut, _all_luts) @@ -276,19 +270,18 @@ public:      {          typename buffer_type::sptr buff;          libusb_zero_copy_mb *front = NULL; +        boost::mutex::scoped_lock lock(_mutex); +        if (_enqueued.empty())          { -            boost::mutex::scoped_lock l(_mutex); -            if (_enqueued.empty()) -            { -                _cond.timed_wait(l, boost::posix_time::microseconds(long(timeout*1e6))); -            } -            if (_enqueued.empty()) return buff; -            front = _enqueued.front(); +            _cond.timed_wait(lock, boost::posix_time::microseconds(long(timeout*1e6)));          } +        if (_enqueued.empty()) return buff; +        front = _enqueued.front(); +        lock.unlock();          buff = front->get_new<buffer_type>(timeout); +        lock.lock(); -        boost::mutex::scoped_lock l(_mutex);          if (buff) _enqueued.pop_front();          this->submit_what_we_can();          return buff; diff --git a/host/lib/usrp/b100/b100_impl.cpp b/host/lib/usrp/b100/b100_impl.cpp index 19df3d6af..305ba42a7 100644 --- a/host/lib/usrp/b100/b100_impl.cpp +++ b/host/lib/usrp/b100/b100_impl.cpp @@ -310,12 +310,14 @@ b100_impl::b100_impl(const device_addr_t &device_addr){      _tree->create<std::string>(rx_codec_path / "name").set("ad9522");      _tree->create<meta_range_t>(rx_codec_path / "gains/pga/range").set(b100_codec_ctrl::rx_pga_gain_range);      _tree->create<double>(rx_codec_path / "gains/pga/value") -        .coerce(boost::bind(&b100_impl::update_rx_codec_gain, this, _1)); +        .coerce(boost::bind(&b100_impl::update_rx_codec_gain, this, _1)) +        .set(0.0);      _tree->create<std::string>(tx_codec_path / "name").set("ad9522");      _tree->create<meta_range_t>(tx_codec_path / "gains/pga/range").set(b100_codec_ctrl::tx_pga_gain_range);      _tree->create<double>(tx_codec_path / "gains/pga/value")          .subscribe(boost::bind(&b100_codec_ctrl::set_tx_pga_gain, _codec_ctrl, _1)) -        .publish(boost::bind(&b100_codec_ctrl::get_tx_pga_gain, _codec_ctrl)); +        .publish(boost::bind(&b100_codec_ctrl::get_tx_pga_gain, _codec_ctrl)) +        .set(0.0);      ////////////////////////////////////////////////////////////////////      // and do the misc mboard sensors diff --git a/host/lib/usrp/b200/b200_iface.cpp b/host/lib/usrp/b200/b200_iface.cpp index 457b380b6..e0809998e 100644 --- a/host/lib/usrp/b200/b200_iface.cpp +++ b/host/lib/usrp/b200/b200_iface.cpp @@ -69,6 +69,11 @@ const static boost::uint8_t FX3_STATE_RUNNING = 0x04;  const static boost::uint8_t FX3_STATE_UNCONFIGURED = 0x05;  const static boost::uint8_t FX3_STATE_ERROR = 0x06; +const static int VREQ_MAX_SIZE_USB2 = 64; +const static int VREQ_MAX_SIZE_USB3 = 512; +const static int VREQ_DEFAULT_SIZE  = VREQ_MAX_SIZE_USB2; +const static int VREQ_MAX_SIZE      = VREQ_MAX_SIZE_USB3; +  typedef boost::uint32_t hash_type; @@ -243,10 +248,12 @@ public:          size_t num_bytes      ){          byte_vector_t recv_bytes(num_bytes); -        fx3_control_read(B200_VREQ_EEPROM_READ, +        int bytes_read = fx3_control_read(B200_VREQ_EEPROM_READ,                           0, offset | (boost::uint16_t(addr) << 8),                           (unsigned char*) &recv_bytes[0],                           num_bytes); +        if (bytes_read != num_bytes) +            throw uhd::io_error("Failed to read data from EEPROM.");          return recv_bytes;      } @@ -492,10 +499,24 @@ public:          hash_type hash = generate_hash(filename);          hash_type loaded_hash; usrp_get_fpga_hash(loaded_hash);          if (hash == loaded_hash) return 0; - -        unsigned char out_buff[64]; -        memset(out_buff, 0x00, sizeof(out_buff)); -        fx3_control_write(B200_VREQ_FPGA_CONFIG, 0, 0, out_buff, 1, 1000); +         +        // Establish default largest possible control request transfer size based on operating USB speed +        int transfer_size = VREQ_DEFAULT_SIZE; +        int current_usb_speed = get_usb_speed(); +        if (current_usb_speed == 3) +            transfer_size = VREQ_MAX_SIZE_USB3; +        else if (current_usb_speed != 2) +            throw uhd::io_error("load_fpga: get_usb_speed returned invalid USB speed (not 2 or 3)."); +         +        UHD_ASSERT_THROW(transfer_size <= VREQ_MAX_SIZE); +         +        unsigned char out_buff[VREQ_MAX_SIZE]; +         +        // Request loopback read, which will indicate the firmware's current control request buffer size +        int nread = fx3_control_read(B200_VREQ_LOOP, 0, 0, out_buff, sizeof(out_buff), 1000); +        if (nread <= 0) +            throw uhd::io_error("load_fpga: unable to complete firmware loopback request."); +        transfer_size = std::min(transfer_size, nread); // Select the smaller value          size_t file_size = 0;          { @@ -510,6 +531,9 @@ public:              throw uhd::io_error("load_fpga: cannot open FPGA input file.");          } +        memset(out_buff, 0x00, sizeof(out_buff)); +        fx3_control_write(B200_VREQ_FPGA_CONFIG, 0, 0, out_buff, 1, 1000); +          wait_count = 0;          do {              fx3_state = get_fx3_status(); @@ -543,14 +567,18 @@ public:          size_t bytes_sent = 0;          while(!file.eof()) { -            file.read((char *) out_buff, sizeof(out_buff)); +            file.read((char *) out_buff, transfer_size);              const std::streamsize n = file.gcount();              if(n == 0) continue;              boost::uint16_t transfer_count = boost::uint16_t(n);              /* Send the data to the device. */ -            fx3_control_write(B200_VREQ_FPGA_DATA, 0, 0, out_buff, transfer_count, 5000); +            int nwritten = fx3_control_write(B200_VREQ_FPGA_DATA, 0, 0, out_buff, transfer_count, 5000); +            if (nwritten <= 0) +                throw uhd::io_error("load_fpga: cannot write bitstream to FX3."); +            else if (nwritten != transfer_count) +                throw uhd::io_error("load_fpga: short write while transferring bitstream to FX3.");              if (load_img_msg)              { diff --git a/host/lib/usrp/b200/b200_iface.hpp b/host/lib/usrp/b200/b200_iface.hpp index 1247d1f86..5b6eeede4 100644 --- a/host/lib/usrp/b200/b200_iface.hpp +++ b/host/lib/usrp/b200/b200_iface.hpp @@ -25,7 +25,17 @@  #include <boost/utility.hpp>  #include "ad9361_ctrl.hpp" -class b200_iface: boost::noncopyable, public virtual uhd::i2c_iface, +const static boost::uint16_t B200_VENDOR_ID  = 0x2500; +const static boost::uint16_t B200_PRODUCT_ID = 0x0020; +const static boost::uint16_t FX3_VID = 0x04b4; +const static boost::uint16_t FX3_DEFAULT_PID = 0x00f3; +const static boost::uint16_t FX3_REENUM_PID = 0x00f0; + +static const std::string     B200_FW_FILE_NAME = "usrp_b200_fw.hex"; +static const std::string     B200_FPGA_FILE_NAME = "usrp_b200_fpga.bin"; +static const std::string     B210_FPGA_FILE_NAME = "usrp_b210_fpga.bin"; + +class UHD_API b200_iface: boost::noncopyable, public virtual uhd::i2c_iface,                    public ad9361_ctrl_iface_type {  public:      typedef boost::shared_ptr<b200_iface> sptr; @@ -64,6 +74,10 @@ public:      //! send SPI through the FX3      virtual void transact_spi( unsigned char *tx_data, size_t num_tx_bits, \              unsigned char *rx_data, size_t num_rx_bits) = 0; + +    virtual void write_eeprom(boost::uint16_t addr, boost::uint16_t offset, const uhd::byte_vector_t &bytes) = 0; + +    virtual uhd::byte_vector_t read_eeprom(boost::uint16_t addr, boost::uint16_t offset, size_t num_bytes) = 0;  }; diff --git a/host/lib/usrp/b200/b200_impl.cpp b/host/lib/usrp/b200/b200_impl.cpp index 0da388b93..132b1198d 100644 --- a/host/lib/usrp/b200/b200_impl.cpp +++ b/host/lib/usrp/b200/b200_impl.cpp @@ -37,10 +37,6 @@ using namespace uhd;  using namespace uhd::usrp;  using namespace uhd::transport; -const boost::uint16_t B200_VENDOR_ID  = 0x2500; -const boost::uint16_t B200_PRODUCT_ID = 0x0020; -const boost::uint16_t INIT_PRODUCT_ID = 0x00f0; -  static const boost::posix_time::milliseconds REENUMERATION_TIMEOUT_MS(3000);  //! mapping of frontend to radio perif index @@ -99,7 +95,7 @@ static device_addrs_t b200_find(const device_addr_t &hint)          catch(const uhd::exception &){continue;} //ignore claimed          //check if fw was already loaded -        if (handle->get_manufacturer() != "Ettus Research LLC") +        if (!(handle->firmware_loaded()))          {              b200_iface::make(control)->load_firmware(b200_fw_image);          } @@ -160,8 +156,15 @@ b200_impl::b200_impl(const device_addr_t &device_addr)      const fs_path mb_path = "/mboards/0";      //try to match the given device address with something on the USB bus +    uint16_t vid = B200_VENDOR_ID; +    uint16_t pid = B200_PRODUCT_ID; +    if (device_addr.has_key("vid")) +            sscanf(device_addr.get("vid").c_str(), "%x", &vid); +    if (device_addr.has_key("pid")) +            sscanf(device_addr.get("pid").c_str(), "%x", &pid); +      std::vector<usb_device_handle::sptr> device_list = -        usb_device_handle::get_device_list(B200_VENDOR_ID, B200_PRODUCT_ID); +        usb_device_handle::get_device_list(vid, pid);      //locate the matching handle in the device list      usb_device_handle::sptr handle; @@ -252,7 +255,7 @@ b200_impl::b200_impl(const device_addr_t &device_addr)      ////////////////////////////////////////////////////////////////////      _async_task_data.reset(new AsyncTaskData());      _async_task_data->async_md.reset(new async_md_type(1000/*messages deep*/)); -    _async_task = uhd::task::make(boost::bind(&b200_impl::handle_async_task, this, _ctrl_transport, _async_task_data)); +    _async_task = uhd::msg_task::make(boost::bind(&b200_impl::handle_async_task, this, _ctrl_transport, _async_task_data));      ////////////////////////////////////////////////////////////////////      // Local control endpoint @@ -474,7 +477,7 @@ b200_impl::b200_impl(const device_addr_t &device_addr)  b200_impl::~b200_impl(void)  { -    UHD_SAFE_CALL +	UHD_SAFE_CALL      (          _async_task.reset();      ) @@ -612,7 +615,7 @@ void b200_impl::setup_radio(const size_t dspno)  /***********************************************************************   * loopback tests   **********************************************************************/ -  +  void b200_impl::register_loopback_self_test(wb_iface::sptr iface)  {      bool test_fail = false; diff --git a/host/lib/usrp/b200/b200_impl.hpp b/host/lib/usrp/b200/b200_impl.hpp index eced4a539..362c45347 100644 --- a/host/lib/usrp/b200/b200_impl.hpp +++ b/host/lib/usrp/b200/b200_impl.hpp @@ -44,10 +44,6 @@  #include <uhd/transport/bounded_buffer.hpp>  #include <boost/weak_ptr.hpp>  #include "recv_packet_demuxer_3000.hpp" - -static const std::string     B200_FW_FILE_NAME = "usrp_b200_fw.hex"; -static const std::string     B200_FPGA_FILE_NAME = "usrp_b200_fpga.bin"; -static const std::string     B210_FPGA_FILE_NAME = "usrp_b210_fpga.bin";  static const boost::uint8_t  B200_FW_COMPAT_NUM_MAJOR = 0x03;  static const boost::uint8_t  B200_FW_COMPAT_NUM_MINOR = 0x00;  static const boost::uint16_t B200_FPGA_COMPAT_NUM = 0x02; @@ -120,7 +116,7 @@ struct b200_impl : public uhd::device      boost::weak_ptr<uhd::tx_streamer> _tx_streamer;      //async ctrl + msgs -    uhd::task::sptr _async_task; +    uhd::msg_task::sptr _async_task;      typedef uhd::transport::bounded_buffer<uhd::async_metadata_t> async_md_type;      struct AsyncTaskData      { @@ -130,7 +126,7 @@ struct b200_impl : public uhd::device          b200_uart::sptr gpsdo_uart;      };      boost::shared_ptr<AsyncTaskData> _async_task_data; -    void handle_async_task(uhd::transport::zero_copy_if::sptr, boost::shared_ptr<AsyncTaskData>); +    boost::optional<uhd::msg_task::msg_type_t> handle_async_task(uhd::transport::zero_copy_if::sptr, boost::shared_ptr<AsyncTaskData>);      void register_loopback_self_test(uhd::wb_iface::sptr iface);      void codec_loopback_self_test(uhd::wb_iface::sptr iface); diff --git a/host/lib/usrp/b200/b200_io_impl.cpp b/host/lib/usrp/b200/b200_io_impl.cpp index d643ef855..4fe90bd4a 100644 --- a/host/lib/usrp/b200/b200_io_impl.cpp +++ b/host/lib/usrp/b200/b200_io_impl.cpp @@ -139,27 +139,44 @@ bool b200_impl::recv_async_msg(      return _async_task_data->async_md->pop_with_timed_wait(async_metadata, timeout);  } -void b200_impl::handle_async_task( +/* + * This method is constantly called in a msg_task loop. + * Incoming messages are dispatched in to the hosts radio_ctrl_cores. + * The radio_ctrl_core queues are accessed via a weak_ptr to them, stored in AsyncTaskData. + * During shutdown the radio_ctrl_core dtor's are called. + * An empty peek32(0) is sent out to flush pending async messages. + * The response to those messages can't be delivered to the ctrl_core queues anymore + * because the shared pointer corresponding to the weak_ptrs is no longer valid. + * Those stranded messages are put into a dump_queue implemented in msg_task. + * A radio_ctrl_core can search for missing messages there. + */ +boost::optional<uhd::msg_task::msg_type_t> b200_impl::handle_async_task(      uhd::transport::zero_copy_if::sptr xport,      boost::shared_ptr<AsyncTaskData> data  )  {      managed_recv_buffer::sptr buff = xport->get_recv_buff(); -    if (not buff or buff->size() < 8) return; +    if (not buff or buff->size() < 8) +        return NULL; +      const boost::uint32_t sid = uhd::wtohx(buff->cast<const boost::uint32_t *>()[1]); -    switch (sid) -    { +    switch (sid) {      //if the packet is a control response      case B200_RESP0_MSG_SID:      case B200_RESP1_MSG_SID:      case B200_LOCAL_RESP_SID:      { -        radio_ctrl_core_3000::sptr ctrl; +    	radio_ctrl_core_3000::sptr ctrl;          if (sid == B200_RESP0_MSG_SID) ctrl = data->radio_ctrl[0].lock();          if (sid == B200_RESP1_MSG_SID) ctrl = data->radio_ctrl[1].lock();          if (sid == B200_LOCAL_RESP_SID) ctrl = data->local_ctrl.lock(); -        if (ctrl) ctrl->push_response(buff->cast<const boost::uint32_t *>()); +        if (ctrl){ +        	ctrl->push_response(buff->cast<const boost::uint32_t *>()); +        } +        else{ +            return std::make_pair(sid, uhd::msg_task::buff_to_vector(buff->cast<boost::uint8_t *>(), buff->size() ) ); +        }          break;      } @@ -204,6 +221,7 @@ void b200_impl::handle_async_task(      default:          UHD_MSG(error) << "Got a ctrl packet with unknown SID " << sid << std::endl;      } +    return NULL;  }  /*********************************************************************** @@ -231,7 +249,7 @@ rx_streamer::sptr b200_impl::get_rx_stream(const uhd::stream_args_t &args_)          //calculate packet size          static const size_t hdr_size = 0              + vrt::max_if_hdr_words32*sizeof(boost::uint32_t) -            + sizeof(vrt::if_packet_info_t().tlr) //forced to have trailer +            //+ sizeof(vrt::if_packet_info_t().tlr) //forced to have trailer              - sizeof(vrt::if_packet_info_t().cid) //no class id ever used              - sizeof(vrt::if_packet_info_t().tsi) //no int time ever used          ; diff --git a/host/lib/usrp/cores/radio_ctrl_core_3000.cpp b/host/lib/usrp/cores/radio_ctrl_core_3000.cpp index 5298fd213..0d6e1c665 100644 --- a/host/lib/usrp/cores/radio_ctrl_core_3000.cpp +++ b/host/lib/usrp/cores/radio_ctrl_core_3000.cpp @@ -35,35 +35,27 @@ using namespace uhd::transport;  static const double ACK_TIMEOUT = 2.0; //supposed to be worst case practical timeout  static const double MASSIVE_TIMEOUT = 10.0; //for when we wait on a timed command -static const size_t SR_READBACK  = 32; +static const size_t SR_READBACK = 32; -class radio_ctrl_core_3000_impl : public radio_ctrl_core_3000 +class radio_ctrl_core_3000_impl: public radio_ctrl_core_3000  {  public: -    radio_ctrl_core_3000_impl( -        const bool big_endian, -        uhd::transport::zero_copy_if::sptr ctrl_xport, -        uhd::transport::zero_copy_if::sptr resp_xport, -        const boost::uint32_t sid, -        const std::string &name -    ): -        _link_type(vrt::if_packet_info_t::LINK_TYPE_CHDR), -        _packet_type(vrt::if_packet_info_t::PACKET_TYPE_CONTEXT), -        _bige(big_endian), -        _ctrl_xport(ctrl_xport), -        _resp_xport(resp_xport), -        _sid(sid), -        _name(name), -        _seq_out(0), -        _timeout(ACK_TIMEOUT), -        _resp_queue(128/*max response msgs*/), -        _resp_queue_size(_resp_xport? _resp_xport->get_num_recv_frames() : 3) +    radio_ctrl_core_3000_impl(const bool big_endian, +            uhd::transport::zero_copy_if::sptr ctrl_xport, +            uhd::transport::zero_copy_if::sptr resp_xport, +            const boost::uint32_t sid, const std::string &name) : +            _link_type(vrt::if_packet_info_t::LINK_TYPE_CHDR), _packet_type( +                    vrt::if_packet_info_t::PACKET_TYPE_CONTEXT), _bige( +                    big_endian), _ctrl_xport(ctrl_xport), _resp_xport( +                    resp_xport), _sid(sid), _name(name), _seq_out(0), _timeout( +                    ACK_TIMEOUT), _resp_queue(128/*max response msgs*/), _resp_queue_size( +                    _resp_xport ? _resp_xport->get_num_recv_frames() : 3)      { -        UHD_LOG << "radio_ctrl_core_3000_impl() " << _name << std::endl; +        UHD_LOG<< "radio_ctrl_core_3000_impl() " << _name << std::endl;          if (resp_xport)          { -            while (resp_xport->get_recv_buff(0.0)){} //flush +            while (resp_xport->get_recv_buff(0.0)) {} //flush          }          this->set_time(uhd::time_spec_t(0.0));          this->set_tick_rate(1.0); //something possible but bogus @@ -74,8 +66,8 @@ public:          UHD_LOG << "~radio_ctrl_core_3000_impl() " << _name << std::endl;          _timeout = ACK_TIMEOUT; //reset timeout to something small          UHD_SAFE_CALL( -            this->peek32(0); //dummy peek with the purpose of ack'ing all packets -            _async_task.reset(); //now its ok to release the task +            this->peek32(0);//dummy peek with the purpose of ack'ing all packets +            _async_task.reset();//now its ok to release the task          )      } @@ -95,7 +87,6 @@ public:      {          boost::mutex::scoped_lock lock(_mutex);          UHD_LOGV(always) << _name << std::hex << " addr 0x" << addr << std::dec << std::endl; -          this->send_pkt(SR_READBACK, addr/8);          this->wait_for_ack(false); @@ -136,6 +127,11 @@ public:      }  private: +    // This is the buffer type for messages in radio control core. +    struct resp_buff_type +    { +        boost::uint32_t data[8]; +    };      /*******************************************************************       * Primary control and interaction private methods @@ -143,7 +139,7 @@ private:      UHD_INLINE void send_pkt(const boost::uint32_t addr, const boost::uint32_t data = 0)      {          managed_send_buffer::sptr buff = _ctrl_xport->get_send_buff(0.0); -        if (not buff){ +        if (not buff) {              throw uhd::runtime_error("fifo ctrl timed out getting a send buffer");          }          boost::uint32_t *pkt = buff->cast<boost::uint32_t *>(); @@ -173,12 +169,11 @@ private:          pkt[packet_info.num_header_words32+0] = (_bige)? uhd::htonx(addr) : uhd::htowx(addr);          pkt[packet_info.num_header_words32+1] = (_bige)? uhd::htonx(data) : uhd::htowx(data);          //UHD_MSG(status) << boost::format("0x%08x, 0x%08x\n") % addr % data; -          //send the buffer over the interface          _outstanding_seqs.push(_seq_out);          buff->commit(sizeof(boost::uint32_t)*(packet_info.num_packet_words32)); -        _seq_out++; //inc seq for next call +        _seq_out++;//inc seq for next call      }      UHD_INLINE boost::uint64_t wait_for_ack(const bool readback) @@ -186,7 +181,6 @@ private:          while (readback or (_outstanding_seqs.size() >= _resp_queue_size))          {              UHD_LOGV(always) << _name << " wait_for_ack: " << "readback = " << readback << " outstanding_seqs.size() " << _outstanding_seqs.size() << std::endl; -              //get seq to ack from outstanding packets list              UHD_ASSERT_THROW(not _outstanding_seqs.empty());              const size_t seq_to_ack = _outstanding_seqs.front(); @@ -218,7 +212,27 @@ private:              //get buffer from response endpoint - or die in timeout              else              { -                UHD_ASSERT_THROW(_resp_queue.pop_with_timed_wait(resp_buff, _timeout)); +                /* +                 * Couldn't get message with haste. +                 * Now check both possible queues for messages. +                 * Messages should come in on _resp_queue, +                 * but could end up in dump_queue. +                 * If we don't get a message --> Die in timeout. +                 */ +                double accum_timeout = 0.0; +                const double short_timeout = 0.005; // == 5ms +                while(not (_resp_queue.pop_with_haste(resp_buff) +                        || check_dump_queue(resp_buff) +                        || _resp_queue.pop_with_timed_wait(resp_buff, short_timeout) +                        )){ +                    /* +                     * If a message couldn't be received within a given timeout +                     * --> throw AssertionError! +                     */ +                    accum_timeout += short_timeout; +                    UHD_ASSERT_THROW(accum_timeout < _timeout); +                } +                  pkt = resp_buff.data;                  packet_info.num_packet_words32 = sizeof(resp_buff)/sizeof(boost::uint32_t);              } @@ -262,9 +276,33 @@ private:                  return ((hi << 32) | lo);              }          } +          return 0;      } +    /* +     * If ctrl_core waits for a message that didn't arrive it can search for it in the dump queue. +     * This actually happens during shutdown. +     * handle_async_task can't access radio_ctrl_cores queue anymore thus it returns the corresponding message. +     * msg_task class implements a dump_queue to store such messages. +     * With check_dump_queue we can check if a message we are waiting for got stranded there. +     * If a message got stuck we get it here and push it onto our own message_queue. +     */ +    bool check_dump_queue(resp_buff_type b) { +        boost::uint32_t recv_sid = (((_sid)<<16)|((_sid)>>16)); +        uhd::msg_task::msg_payload_t msg; +        do{ +            msg = _async_task->get_msg_from_dump_queue(recv_sid); +        } +        while(msg.size() < 8 && msg.size() != 0); + +        if(msg.size() >= 8) { +            memcpy(b.data, &msg.front(), 8); +            return true; +        } +        return false; +    } +      void push_response(const boost::uint32_t *buff)      {          resp_buff_type resp_buff; @@ -272,7 +310,7 @@ private:          _resp_queue.push_with_haste(resp_buff);      } -    void hold_task(boost::shared_ptr<void> task) +    void hold_task(uhd::msg_task::sptr task)      {          _async_task = task;      } @@ -282,7 +320,7 @@ private:      const bool _bige;      const uhd::transport::zero_copy_if::sptr _ctrl_xport;      const uhd::transport::zero_copy_if::sptr _resp_xport; -    boost::shared_ptr<void> _async_task; +    uhd::msg_task::sptr _async_task;      const boost::uint32_t _sid;      const std::string _name;      boost::mutex _mutex; @@ -292,22 +330,15 @@ private:      double _tick_rate;      double _timeout;      std::queue<size_t> _outstanding_seqs; -    struct resp_buff_type -    { -        boost::uint32_t data[8]; -    };      bounded_buffer<resp_buff_type> _resp_queue;      const size_t _resp_queue_size;  }; - -radio_ctrl_core_3000::sptr radio_ctrl_core_3000::make( -    const bool big_endian, -    zero_copy_if::sptr ctrl_xport, -    zero_copy_if::sptr resp_xport, -    const boost::uint32_t sid, -    const std::string &name -) +radio_ctrl_core_3000::sptr radio_ctrl_core_3000::make(const bool big_endian, +        zero_copy_if::sptr ctrl_xport, zero_copy_if::sptr resp_xport, +        const boost::uint32_t sid, const std::string &name)  { -    return sptr(new radio_ctrl_core_3000_impl(big_endian, ctrl_xport, resp_xport, sid, name)); +    return sptr( +            new radio_ctrl_core_3000_impl(big_endian, ctrl_xport, resp_xport, +                    sid, name));  } diff --git a/host/lib/usrp/cores/radio_ctrl_core_3000.hpp b/host/lib/usrp/cores/radio_ctrl_core_3000.hpp index a49ca2a4b..51a307c10 100644 --- a/host/lib/usrp/cores/radio_ctrl_core_3000.hpp +++ b/host/lib/usrp/cores/radio_ctrl_core_3000.hpp @@ -18,11 +18,12 @@  #ifndef INCLUDED_LIBUHD_USRP_RADIO_CTRL_3000_HPP  #define INCLUDED_LIBUHD_USRP_RADIO_CTRL_3000_HPP +#include <uhd/utils/msg_task.hpp>  #include <uhd/types/time_spec.hpp>  #include <uhd/transport/zero_copy.hpp> +#include <uhd/types/wb_iface.hpp>  #include <boost/shared_ptr.hpp>  #include <boost/utility.hpp> -#include <uhd/types/wb_iface.hpp>  #include <string>  /*! @@ -43,7 +44,7 @@ public:      );      //! Hold a ref to a task thats feeding push response -    virtual void hold_task(boost::shared_ptr<void> task) = 0; +    virtual void hold_task(uhd::msg_task::sptr task) = 0;      //! Push a response externall (resp_xport is NULL)      virtual void push_response(const boost::uint32_t *buff) = 0; diff --git a/host/lib/usrp/dboard/db_dbsrx2.cpp b/host/lib/usrp/dboard/db_dbsrx2.cpp index 013f3178a..8a8f61a69 100644 --- a/host/lib/usrp/dboard/db_dbsrx2.cpp +++ b/host/lib/usrp/dboard/db_dbsrx2.cpp @@ -358,12 +358,12 @@ double dbsrx2::set_gain(double gain, const std::string &name){   * Bandwidth Handling   **********************************************************************/  double dbsrx2::set_bandwidth(double bandwidth){ -    //convert complex bandpass to lowpass bandwidth -    bandwidth = bandwidth/2.0; -      //clip the input      bandwidth = dbsrx2_bandwidth_range.clip(bandwidth); +    //convert complex bandpass to lowpass bandwidth +    bandwidth = bandwidth/2.0; +      _max2112_write_regs.lp = int((bandwidth/1e6 - 4)/0.29 + 12);      _bandwidth = double(4 + (_max2112_write_regs.lp - 12) * 0.29)*1e6; diff --git a/host/lib/usrp/dboard/db_sbx_common.cpp b/host/lib/usrp/dboard/db_sbx_common.cpp index 9db29e65a..5b713c6d7 100644 --- a/host/lib/usrp/dboard/db_sbx_common.cpp +++ b/host/lib/usrp/dboard/db_sbx_common.cpp @@ -21,6 +21,137 @@ using namespace uhd;  using namespace uhd::usrp;  using namespace boost::assign; +/*********************************************************************** + * ADF 4350/4351 Tuning Utility + **********************************************************************/ +sbx_xcvr::sbx_versionx::adf435x_tuning_settings sbx_xcvr::sbx_versionx::_tune_adf435x_synth( +    double target_freq, +    double ref_freq, +    const adf435x_tuning_constraints& constraints, +    double& actual_freq) +{ +    //Default invalid value for actual_freq +    actual_freq = 0; + +    double pfd_freq = 0; +    boost::uint16_t R = 0, BS = 0, N = 0, FRAC = 0, MOD = 0; +    boost::uint16_t RFdiv = static_cast<boost::uint16_t>(constraints.rf_divider_range.start()); +    bool D = false, T = false; + +    //Reference doubler for 50% duty cycle +    //If ref_freq < 12.5MHz enable the reference doubler +    D = (ref_freq <= constraints.ref_doubler_threshold); + +    static const double MIN_VCO_FREQ = 2.2e9; +    static const double MAX_VCO_FREQ = 4.4e9; + +    //increase RF divider until acceptable VCO frequency +    double vco_freq = target_freq; +    while (vco_freq < MIN_VCO_FREQ && RFdiv < static_cast<boost::uint16_t>(constraints.rf_divider_range.stop())) { +        vco_freq *= 2; +        RFdiv *= 2; +    } + +    /* +     * The goal here is to loop though possible R dividers, +     * band select clock dividers, N (int) dividers, and FRAC +     * (frac) dividers. +     * +     * Calculate the N and F dividers for each set of values. +     * The loop exits when it meets all of the constraints. +     * The resulting loop values are loaded into the registers. +     * +     * from pg.21 +     * +     * f_pfd = f_ref*(1+D)/(R*(1+T)) +     * f_vco = (N + (FRAC/MOD))*f_pfd +     *    N = f_vco/f_pfd - FRAC/MOD = f_vco*((R*(T+1))/(f_ref*(1+D))) - FRAC/MOD +     * f_rf = f_vco/RFdiv) +     * f_actual = f_rf/2 +     */ +    for(R = 1; R <= 1023; R+=1){ +        //PFD input frequency = f_ref/R ... ignoring Reference doubler/divide-by-2 (D & T) +        pfd_freq = ref_freq*(D?2:1)/(R*(T?2:1)); + +        //keep the PFD frequency at or below 25MHz (Loop Filter Bandwidth) +        if (pfd_freq > constraints.pfd_freq_max) continue; + +        //ignore fractional part of tuning +        //N is computed from target_freq and not vco_freq because the feedback +        //mode is set to FEEDBACK_SELECT_DIVIDED +        N = boost::uint16_t(std::floor(target_freq/pfd_freq)); + +        //keep N > minimum int divider requirement +        if (N < static_cast<boost::uint16_t>(constraints.int_range.start())) continue; + +        for(BS=1; BS <= 255; BS+=1){ +            //keep the band select frequency at or below band_sel_freq_max +            //constraint on band select clock +            if (pfd_freq/BS > constraints.band_sel_freq_max) continue; +            goto done_loop; +        } +    } done_loop: + +    //Fractional-N calculation +    MOD = 4095; //max fractional accuracy +    //N is computed from target_freq and not vco_freq because the feedback +    //mode is set to FEEDBACK_SELECT_DIVIDED +    FRAC = static_cast<boost::uint16_t>((target_freq/pfd_freq - N)*MOD); +    if (constraints.force_frac0) { +        if (FRAC > (MOD / 2)) { //Round integer such that actual freq is closest to target +            N++; +        } +        FRAC = 0; +    } + +    //Reference divide-by-2 for 50% duty cycle +    // if R even, move one divide by 2 to to regs.reference_divide_by_2 +    if(R % 2 == 0) { +        T = true; +        R /= 2; +    } + +    //Typical phase resync time documented in data sheet pg.24 +    static const double PHASE_RESYNC_TIME = 400e-6; + +    //actual frequency calculation +    actual_freq = double((N + (double(FRAC)/double(MOD)))*ref_freq*(D?2:1)/(R*(T?2:1))); + +    //load the settings +    adf435x_tuning_settings settings; +    settings.frac_12_bit = FRAC; +    settings.int_16_bit = N; +    settings.mod_12_bit = MOD; +    settings.clock_divider_12_bit = std::max<boost::uint16_t>(1, std::ceil(PHASE_RESYNC_TIME*pfd_freq/MOD)); +    settings.r_counter_10_bit = R; +    settings.r_divide_by_2_en = T; +    settings.r_doubler_en = D; +    settings.band_select_clock_div = BS; +    settings.rf_divider = RFdiv; +    settings.feedback_after_divider = true; + +    UHD_LOGV(often) +        << boost::format("ADF 435X Frequencies (MHz): REQUESTED=%0.9f, ACTUAL=%0.9f" +        ) % (target_freq/1e6) % (actual_freq/1e6) << std::endl +        << boost::format("ADF 435X Intermediates (MHz): VCO=%0.2f, PFD=%0.2f, BAND=%0.2f, REF=%0.2f" +        ) % (vco_freq/1e6) % (pfd_freq/1e6) % (pfd_freq/BS/1e6) % (ref_freq/1e6) << std::endl +        << boost::format("ADF 435X Settings: R=%d, BS=%d, N=%d, FRAC=%d, MOD=%d, T=%d, D=%d, RFdiv=%d" +        ) % R % BS % N % FRAC % MOD % T % D % RFdiv << std::endl; + +    UHD_ASSERT_THROW((settings.frac_12_bit          & ((boost::uint16_t)~0xFFF)) == 0); +    UHD_ASSERT_THROW((settings.mod_12_bit           & ((boost::uint16_t)~0xFFF)) == 0); +    UHD_ASSERT_THROW((settings.clock_divider_12_bit & ((boost::uint16_t)~0xFFF)) == 0); +    UHD_ASSERT_THROW((settings.r_counter_10_bit     & ((boost::uint16_t)~0x3FF)) == 0); + +    UHD_ASSERT_THROW(vco_freq >= MIN_VCO_FREQ and vco_freq <= MAX_VCO_FREQ); +    UHD_ASSERT_THROW(settings.rf_divider >= static_cast<boost::uint16_t>(constraints.rf_divider_range.start())); +    UHD_ASSERT_THROW(settings.rf_divider <= static_cast<boost::uint16_t>(constraints.rf_divider_range.stop())); +    UHD_ASSERT_THROW(settings.int_16_bit >= static_cast<boost::uint16_t>(constraints.int_range.start())); +    UHD_ASSERT_THROW(settings.int_16_bit <= static_cast<boost::uint16_t>(constraints.int_range.stop())); + +    return settings; +} +  /***********************************************************************   * Register the SBX dboard (min freq, max freq, rx div2, tx div2) @@ -362,4 +493,3 @@ void sbx_xcvr::flash_leds(void) {      this->get_iface()->set_gpio_ddr(dboard_iface::UNIT_TX, (TXIO_MASK|TX_LED_IO));      this->get_iface()->set_gpio_ddr(dboard_iface::UNIT_RX, (RXIO_MASK|RX_LED_IO));  } - diff --git a/host/lib/usrp/dboard/db_sbx_common.hpp b/host/lib/usrp/dboard/db_sbx_common.hpp index 4f3a2eeaa..e9bb2434c 100644 --- a/host/lib/usrp/dboard/db_sbx_common.hpp +++ b/host/lib/usrp/dboard/db_sbx_common.hpp @@ -181,6 +181,34 @@ protected:          ~sbx_versionx(void) {}          virtual double set_lo_freq(dboard_iface::unit_t unit, double target_freq) = 0; +    protected: +        struct adf435x_tuning_constraints { +            bool            force_frac0; +            double          ref_doubler_threshold; +            double          pfd_freq_max; +            double          band_sel_freq_max; +            uhd::range_t    rf_divider_range; +            uhd::range_t    int_range; +        }; + +        struct adf435x_tuning_settings { +            boost::uint16_t frac_12_bit; +            boost::uint16_t int_16_bit; +            boost::uint16_t mod_12_bit; +            boost::uint16_t r_counter_10_bit; +            bool            r_doubler_en; +            bool            r_divide_by_2_en; +            boost::uint16_t clock_divider_12_bit; +            boost::uint8_t  band_select_clock_div; +            boost::uint16_t rf_divider; +            bool            feedback_after_divider; +        }; + +        adf435x_tuning_settings _tune_adf435x_synth( +            double target_freq, +            double ref_freq, +            const adf435x_tuning_constraints& constraints, +            double& actual_freq);      };      /*! diff --git a/host/lib/usrp/dboard/db_sbx_version3.cpp b/host/lib/usrp/dboard/db_sbx_version3.cpp index 2765d530c..b0c9cd18f 100644 --- a/host/lib/usrp/dboard/db_sbx_version3.cpp +++ b/host/lib/usrp/dboard/db_sbx_version3.cpp @@ -63,85 +63,21 @@ double sbx_xcvr::sbx_version3::set_lo_freq(dboard_iface::unit_t unit, double tar          (16, adf4350_regs_t::RF_DIVIDER_SELECT_DIV16)      ; -    double actual_freq, pfd_freq; -    double ref_freq = self_base->get_iface()->get_clock_rate(unit); -    int R=0, BS=0, N=0, FRAC=0, MOD=0; -    int RFdiv = 1; -    adf4350_regs_t::reference_divide_by_2_t T     = adf4350_regs_t::REFERENCE_DIVIDE_BY_2_DISABLED; -    adf4350_regs_t::reference_doubler_t     D     = adf4350_regs_t::REFERENCE_DOUBLER_DISABLED;     - -    //Reference doubler for 50% duty cycle -    // if ref_freq < 12.5MHz enable regs.reference_divide_by_2 -    if(ref_freq <= 12.5e6) D = adf4350_regs_t::REFERENCE_DOUBLER_ENABLED; - -    //increase RF divider until acceptable VCO frequency -    double vco_freq = target_freq; -    while (vco_freq < 2.2e9) { -        vco_freq *= 2; -        RFdiv *= 2; -    } -      //use 8/9 prescaler for vco_freq > 3 GHz (pg.18 prescaler)      adf4350_regs_t::prescaler_t prescaler = target_freq > 3e9 ? adf4350_regs_t::PRESCALER_8_9 : adf4350_regs_t::PRESCALER_4_5; -    /* -     * The goal here is to loop though possible R dividers, -     * band select clock dividers, N (int) dividers, and FRAC  -     * (frac) dividers. -     * -     * Calculate the N and F dividers for each set of values. -     * The loop exits when it meets all of the constraints. -     * The resulting loop values are loaded into the registers. -     * -     * from pg.21 -     * -     * f_pfd = f_ref*(1+D)/(R*(1+T)) -     * f_vco = (N + (FRAC/MOD))*f_pfd -     *    N = f_vco/f_pfd - FRAC/MOD = f_vco*((R*(T+1))/(f_ref*(1+D))) - FRAC/MOD -     * f_rf = f_vco/RFdiv) -     * f_actual = f_rf/2 -     */ -    for(R = 1; R <= 1023; R+=1){ -        //PFD input frequency = f_ref/R ... ignoring Reference doubler/divide-by-2 (D & T) -        pfd_freq = ref_freq*(1+D)/(R*(1+T)); - -        //keep the PFD frequency at or below 25MHz (Loop Filter Bandwidth) -        if (pfd_freq > 25e6) continue; - -        //ignore fractional part of tuning -        N = int(std::floor(target_freq/pfd_freq)); - -        //keep N > minimum int divider requirement -        if (N < prescaler_to_min_int_div[prescaler]) continue; - -        for(BS=1; BS <= 255; BS+=1){ -            //keep the band select frequency at or below 100KHz -            //constraint on band select clock -            if (pfd_freq/BS > 100e3) continue; -            goto done_loop; -        } -    } done_loop: - -    //Fractional-N calculation -    MOD = 4095; //max fractional accuracy -    FRAC = int((target_freq/pfd_freq - N)*MOD); - -    //Reference divide-by-2 for 50% duty cycle -    // if R even, move one divide by 2 to to regs.reference_divide_by_2 -    if(R % 2 == 0){ -        T = adf4350_regs_t::REFERENCE_DIVIDE_BY_2_ENABLED; -        R /= 2; -    } - -    //actual frequency calculation -    actual_freq = double((N + (double(FRAC)/double(MOD)))*ref_freq*(1+int(D))/(R*(1+int(T)))); +    adf435x_tuning_constraints tuning_constraints; +    tuning_constraints.force_frac0 = false; +    tuning_constraints.band_sel_freq_max = 100e3; +    tuning_constraints.ref_doubler_threshold = 12.5e6; +    tuning_constraints.int_range = uhd::range_t(prescaler_to_min_int_div[prescaler], 4095);  //INT is a 12-bit field +    tuning_constraints.pfd_freq_max = 25e6; +    tuning_constraints.rf_divider_range = uhd::range_t(1, 16); -    UHD_LOGV(often) -        << boost::format("SBX Intermediates: ref=%0.2f, outdiv=%f, fbdiv=%f") % (ref_freq*(1+int(D))/(R*(1+int(T)))) % double(RFdiv*2) % double(N + double(FRAC)/double(MOD)) << std::endl -        << boost::format("SBX tune: R=%d, BS=%d, N=%d, FRAC=%d, MOD=%d, T=%d, D=%d, RFdiv=%d" -            ) % R % BS % N % FRAC % MOD % T % D % RFdiv << std::endl -        << boost::format("SBX Frequencies (MHz): REQ=%0.2f, ACT=%0.2f, VCO=%0.2f, PFD=%0.2f, BAND=%0.2f" -            ) % (target_freq/1e6) % (actual_freq/1e6) % (vco_freq/1e6) % (pfd_freq/1e6) % (pfd_freq/BS/1e6) << std::endl; +    double actual_freq; +    adf435x_tuning_settings tuning_settings = _tune_adf435x_synth( +        target_freq, self_base->get_iface()->get_clock_rate(unit), +        tuning_constraints, actual_freq);      //load the register values      adf4350_regs_t regs; @@ -151,19 +87,25 @@ double sbx_xcvr::sbx_version3::set_lo_freq(dboard_iface::unit_t unit, double tar      else          regs.output_power = adf4350_regs_t::OUTPUT_POWER_5DBM; -    regs.frac_12_bit = FRAC; -    regs.int_16_bit = N; -    regs.mod_12_bit = MOD; -    regs.clock_divider_12_bit = std::max(1, int(std::ceil(400e-6*pfd_freq/MOD))); -    regs.feedback_select = adf4350_regs_t::FEEDBACK_SELECT_DIVIDED; -    regs.clock_div_mode = adf4350_regs_t::CLOCK_DIV_MODE_RESYNC_ENABLE; -    regs.prescaler = prescaler; -    regs.r_counter_10_bit = R; -    regs.reference_divide_by_2 = T; -    regs.reference_doubler = D; -    regs.band_select_clock_div = BS; -    UHD_ASSERT_THROW(rfdivsel_to_enum.has_key(RFdiv)); -    regs.rf_divider_select = rfdivsel_to_enum[RFdiv]; +    regs.frac_12_bit            = tuning_settings.frac_12_bit; +    regs.int_16_bit             = tuning_settings.int_16_bit; +    regs.mod_12_bit             = tuning_settings.mod_12_bit; +    regs.clock_divider_12_bit   = tuning_settings.clock_divider_12_bit; +    regs.feedback_select        = tuning_settings.feedback_after_divider ? +                                    adf4350_regs_t::FEEDBACK_SELECT_DIVIDED : +                                    adf4350_regs_t::FEEDBACK_SELECT_FUNDAMENTAL; +    regs.clock_div_mode         = adf4350_regs_t::CLOCK_DIV_MODE_RESYNC_ENABLE; +    regs.prescaler              = prescaler; +    regs.r_counter_10_bit       = tuning_settings.r_counter_10_bit; +    regs.reference_divide_by_2  = tuning_settings.r_divide_by_2_en ? +                                    adf4350_regs_t::REFERENCE_DIVIDE_BY_2_ENABLED : +                                    adf4350_regs_t::REFERENCE_DIVIDE_BY_2_DISABLED; +    regs.reference_doubler      = tuning_settings.r_doubler_en ? +                                    adf4350_regs_t::REFERENCE_DOUBLER_ENABLED : +                                    adf4350_regs_t::REFERENCE_DOUBLER_DISABLED; +    regs.band_select_clock_div  = tuning_settings.band_select_clock_div; +    UHD_ASSERT_THROW(rfdivsel_to_enum.has_key(tuning_settings.rf_divider)); +    regs.rf_divider_select      = rfdivsel_to_enum[tuning_settings.rf_divider];      //reset the N and R counter      regs.counter_reset = adf4350_regs_t::COUNTER_RESET_ENABLED; diff --git a/host/lib/usrp/dboard/db_sbx_version4.cpp b/host/lib/usrp/dboard/db_sbx_version4.cpp index 27fd68b05..8d95b0655 100644 --- a/host/lib/usrp/dboard/db_sbx_version4.cpp +++ b/host/lib/usrp/dboard/db_sbx_version4.cpp @@ -66,85 +66,21 @@ double sbx_xcvr::sbx_version4::set_lo_freq(dboard_iface::unit_t unit, double tar          (64, adf4351_regs_t::RF_DIVIDER_SELECT_DIV64)      ; -    double actual_freq, pfd_freq; -    double ref_freq = self_base->get_iface()->get_clock_rate(unit); -    int R=0, BS=0, N=0, FRAC=0, MOD=0; -    int RFdiv = 1; -    adf4351_regs_t::reference_divide_by_2_t T     = adf4351_regs_t::REFERENCE_DIVIDE_BY_2_DISABLED; -    adf4351_regs_t::reference_doubler_t     D     = adf4351_regs_t::REFERENCE_DOUBLER_DISABLED;     - -    //Reference doubler for 50% duty cycle -    // if ref_freq < 12.5MHz enable regs.reference_divide_by_2 -    if(ref_freq <= 12.5e6) D = adf4351_regs_t::REFERENCE_DOUBLER_ENABLED; - -    //increase RF divider until acceptable VCO frequency -    double vco_freq = target_freq; -    while (vco_freq < 2.2e9) { -        vco_freq *= 2; -        RFdiv *= 2; -    } -      //use 8/9 prescaler for vco_freq > 3 GHz (pg.18 prescaler) -    adf4351_regs_t::prescaler_t prescaler = target_freq > 3e9 ? adf4351_regs_t::PRESCALER_8_9 : adf4351_regs_t::PRESCALER_4_5; - -    /* -     * The goal here is to loop though possible R dividers, -     * band select clock dividers, N (int) dividers, and FRAC  -     * (frac) dividers. -     * -     * Calculate the N and F dividers for each set of values. -     * The loop exits when it meets all of the constraints. -     * The resulting loop values are loaded into the registers. -     * -     * from pg.21 -     * -     * f_pfd = f_ref*(1+D)/(R*(1+T)) -     * f_vco = (N + (FRAC/MOD))*f_pfd -     *    N = f_vco/f_pfd - FRAC/MOD = f_vco*((R*(T+1))/(f_ref*(1+D))) - FRAC/MOD -     * f_rf = f_vco/RFdiv) -     * f_actual = f_rf/2 -     */ -    for(R = 1; R <= 1023; R+=1){ -        //PFD input frequency = f_ref/R ... ignoring Reference doubler/divide-by-2 (D & T) -        pfd_freq = ref_freq*(1+D)/(R*(1+T)); - -        //keep the PFD frequency at or below 25MHz (Loop Filter Bandwidth) -        if (pfd_freq > 25e6) continue; - -        //ignore fractional part of tuning -        N = int(std::floor(vco_freq/pfd_freq)); - -        //keep N > minimum int divider requirement -        if (N < prescaler_to_min_int_div[prescaler]) continue; - -        for(BS=1; BS <= 255; BS+=1){ -            //keep the band select frequency at or below 100KHz -            //constraint on band select clock -            if (pfd_freq/BS > 100e3) continue; -            goto done_loop; -        } -    } done_loop: - -    //Fractional-N calculation -    MOD = 4095; //max fractional accuracy -    FRAC = int((target_freq/pfd_freq - N)*MOD); - -    //Reference divide-by-2 for 50% duty cycle -    // if R even, move one divide by 2 to to regs.reference_divide_by_2 -    if(R % 2 == 0){ -        T = adf4351_regs_t::REFERENCE_DIVIDE_BY_2_ENABLED; -        R /= 2; -    } +    adf4351_regs_t::prescaler_t prescaler = target_freq > 3.6e9 ? adf4351_regs_t::PRESCALER_8_9 : adf4351_regs_t::PRESCALER_4_5; -    //actual frequency calculation -    actual_freq = double((N + (double(FRAC)/double(MOD)))*ref_freq*(1+int(D))/(R*(1+int(T)))); +    adf435x_tuning_constraints tuning_constraints; +    tuning_constraints.force_frac0 = false; +    tuning_constraints.band_sel_freq_max = 100e3; +    tuning_constraints.ref_doubler_threshold = 12.5e6; +    tuning_constraints.int_range = uhd::range_t(prescaler_to_min_int_div[prescaler], 4095);  //INT is a 12-bit field +    tuning_constraints.pfd_freq_max = 25e6; +    tuning_constraints.rf_divider_range = uhd::range_t(1, 64); -    UHD_LOGV(often) -        << boost::format("SBX Intermediates: ref=%0.2f, outdiv=%f, fbdiv=%f") % (ref_freq*(1+int(D))/(R*(1+int(T)))) % double(RFdiv*2) % double(N + double(FRAC)/double(MOD)) << std::endl -        << boost::format("SBX tune: R=%d, BS=%d, N=%d, FRAC=%d, MOD=%d, T=%d, D=%d, RFdiv=%d" -            ) % R % BS % N % FRAC % MOD % T % D % RFdiv << std::endl -        << boost::format("SBX Frequencies (MHz): REQ=%0.2f, ACT=%0.2f, VCO=%0.2f, PFD=%0.2f, BAND=%0.2f" -            ) % (target_freq/1e6) % (actual_freq/1e6) % (vco_freq/1e6) % (pfd_freq/1e6) % (pfd_freq/BS/1e6) << std::endl; +    double actual_freq; +    adf435x_tuning_settings tuning_settings = _tune_adf435x_synth( +        target_freq, self_base->get_iface()->get_clock_rate(unit), +        tuning_constraints, actual_freq);      //load the register values      adf4351_regs_t regs; @@ -154,19 +90,25 @@ double sbx_xcvr::sbx_version4::set_lo_freq(dboard_iface::unit_t unit, double tar      else          regs.output_power = adf4351_regs_t::OUTPUT_POWER_5DBM; -    regs.frac_12_bit = FRAC; -    regs.int_16_bit = N; -    regs.mod_12_bit = MOD; -    regs.clock_divider_12_bit = std::max(1, int(std::ceil(400e-6*pfd_freq/MOD))); -    regs.feedback_select = adf4351_regs_t::FEEDBACK_SELECT_DIVIDED; -    regs.clock_div_mode = adf4351_regs_t::CLOCK_DIV_MODE_RESYNC_ENABLE; -    regs.prescaler = prescaler; -    regs.r_counter_10_bit = R; -    regs.reference_divide_by_2 = T; -    regs.reference_doubler = D; -    regs.band_select_clock_div = BS; -    UHD_ASSERT_THROW(rfdivsel_to_enum.has_key(RFdiv)); -    regs.rf_divider_select = rfdivsel_to_enum[RFdiv]; +    regs.frac_12_bit            = tuning_settings.frac_12_bit; +    regs.int_16_bit             = tuning_settings.int_16_bit; +    regs.mod_12_bit             = tuning_settings.mod_12_bit; +    regs.clock_divider_12_bit   = tuning_settings.clock_divider_12_bit; +    regs.feedback_select        = tuning_settings.feedback_after_divider ? +                                    adf4351_regs_t::FEEDBACK_SELECT_DIVIDED : +                                    adf4351_regs_t::FEEDBACK_SELECT_FUNDAMENTAL; +    regs.clock_div_mode         = adf4351_regs_t::CLOCK_DIV_MODE_RESYNC_ENABLE; +    regs.prescaler              = prescaler; +    regs.r_counter_10_bit       = tuning_settings.r_counter_10_bit; +    regs.reference_divide_by_2  = tuning_settings.r_divide_by_2_en ? +                                    adf4351_regs_t::REFERENCE_DIVIDE_BY_2_ENABLED : +                                    adf4351_regs_t::REFERENCE_DIVIDE_BY_2_DISABLED; +    regs.reference_doubler      = tuning_settings.r_doubler_en ? +                                    adf4351_regs_t::REFERENCE_DOUBLER_ENABLED : +                                    adf4351_regs_t::REFERENCE_DOUBLER_DISABLED; +    regs.band_select_clock_div  = tuning_settings.band_select_clock_div; +    UHD_ASSERT_THROW(rfdivsel_to_enum.has_key(tuning_settings.rf_divider)); +    regs.rf_divider_select      = rfdivsel_to_enum[tuning_settings.rf_divider];      //reset the N and R counter      regs.counter_reset = adf4351_regs_t::COUNTER_RESET_ENABLED; diff --git a/host/lib/usrp/usrp1/usrp1_impl.cpp b/host/lib/usrp/usrp1/usrp1_impl.cpp index 253ac1d6f..625926f36 100644 --- a/host/lib/usrp/usrp1/usrp1_impl.cpp +++ b/host/lib/usrp/usrp1/usrp1_impl.cpp @@ -258,12 +258,14 @@ usrp1_impl::usrp1_impl(const device_addr_t &device_addr){          _tree->create<std::string>(rx_codec_path / "name").set("ad9522");          _tree->create<meta_range_t>(rx_codec_path / "gains/pga/range").set(usrp1_codec_ctrl::rx_pga_gain_range);          _tree->create<double>(rx_codec_path / "gains/pga/value") -            .coerce(boost::bind(&usrp1_impl::update_rx_codec_gain, this, db, _1)); +            .coerce(boost::bind(&usrp1_impl::update_rx_codec_gain, this, db, _1)) +            .set(0.0);          _tree->create<std::string>(tx_codec_path / "name").set("ad9522");          _tree->create<meta_range_t>(tx_codec_path / "gains/pga/range").set(usrp1_codec_ctrl::tx_pga_gain_range);          _tree->create<double>(tx_codec_path / "gains/pga/value")              .subscribe(boost::bind(&usrp1_codec_ctrl::set_tx_pga_gain, _dbc[db].codec, _1)) -            .publish(boost::bind(&usrp1_codec_ctrl::get_tx_pga_gain, _dbc[db].codec)); +            .publish(boost::bind(&usrp1_codec_ctrl::get_tx_pga_gain, _dbc[db].codec)) +            .set(0.0);      }      //////////////////////////////////////////////////////////////////// @@ -407,7 +409,7 @@ usrp1_impl::usrp1_impl(const device_addr_t &device_addr){          _tree->access<subdev_spec_t>(mb_path / "rx_subdev_spec").set(_rx_subdev_spec);      if (_tree->list(mb_path / "tx_dsps").size() > 0)          _tree->access<subdev_spec_t>(mb_path / "tx_subdev_spec").set(_tx_subdev_spec); -  +  }  usrp1_impl::~usrp1_impl(void){ diff --git a/host/lib/utils/paths.cpp b/host/lib/utils/paths.cpp index f9d8b613c..3e2bea1c6 100644 --- a/host/lib/utils/paths.cpp +++ b/host/lib/utils/paths.cpp @@ -34,8 +34,19 @@  namespace fs = boost::filesystem;  /*********************************************************************** + * Get a list of paths for an environment variable + **********************************************************************/ +static std::string get_env_var(const std::string &var_name, const std::string &def_val = ""){ +    const char *var_value_ptr = std::getenv(var_name.c_str()); +    return (var_value_ptr == NULL)? def_val : var_value_ptr; +} + +static std::vector<fs::path> get_env_paths(const std::string &var_name){ + +/***********************************************************************   * Determine the paths separator   **********************************************************************/ +  #ifdef UHD_PLATFORM_WIN32      static const std::string env_path_sep = ";";  #else @@ -46,16 +57,6 @@ namespace fs = boost::filesystem;      boost::tokenizer<boost::char_separator<char> > \      (inp, boost::char_separator<char>(env_path_sep.c_str())) -/*********************************************************************** - * Get a list of paths for an environment variable - **********************************************************************/ -static std::string get_env_var(const std::string &var_name, const std::string &def_val = ""){ -    const char *var_value_ptr = std::getenv(var_name.c_str()); -    return (var_value_ptr == NULL)? def_val : var_value_ptr; -} - -static std::vector<fs::path> get_env_paths(const std::string &var_name){ -      std::string var_value = get_env_var(var_name);      //convert to filesystem path, filter blank paths @@ -85,6 +86,7 @@ std::vector<fs::path> get_image_paths(void){  std::vector<fs::path> get_module_paths(void){      std::vector<fs::path> paths = get_env_paths("UHD_MODULE_PATH");      paths.push_back(fs::path(uhd::get_pkg_path()) / UHD_LIB_DIR / "uhd" / "modules"); +    paths.push_back(fs::path(uhd::get_pkg_path()) / "share" / "uhd" / "modules");      return paths;  } diff --git a/host/lib/utils/tasks.cpp b/host/lib/utils/tasks.cpp index 1f735de06..08c32a5fb 100644 --- a/host/lib/utils/tasks.cpp +++ b/host/lib/utils/tasks.cpp @@ -16,11 +16,13 @@  //  #include <uhd/utils/tasks.hpp> +#include <uhd/utils/msg_task.hpp>  #include <uhd/utils/msg.hpp>  #include <boost/thread/thread.hpp>  #include <boost/thread/barrier.hpp>  #include <exception>  #include <iostream> +#include <vector>  using namespace uhd; @@ -80,3 +82,100 @@ private:  task::sptr task::make(const task_fcn_type &task_fcn){      return task::sptr(new task_impl(task_fcn));  } + +/* + * During shutdown pointers to queues for radio_ctrl_core might not be available anymore. + * msg_task_impl provides a dump_queue for such messages. + * ctrl_cores can check this queue for stranded messages. + */ + +class msg_task_impl : public msg_task{ +public: + +    msg_task_impl(const task_fcn_type &task_fcn): +        _spawn_barrier(2) +    { +        _thread_group.create_thread(boost::bind(&msg_task_impl::task_loop, this, task_fcn)); +        _spawn_barrier.wait(); +    } + +    ~msg_task_impl(void){ +        _running = false; +        _thread_group.interrupt_all(); +        _thread_group.join_all(); +    } + +    /* +     * Returns the first message for the given SID. +     * This way a radio_ctrl_core doesn't have to die in timeout but can check for stranded messages here. +     * This might happen during shutdown when dtors are called. +     * See also: comments in b200_io_impl->handle_async_task +     */ +    msg_payload_t get_msg_from_dump_queue(boost::uint32_t sid) +    { +        boost::mutex::scoped_lock lock(_mutex); +        msg_payload_t b; +        for (size_t i = 0; i < _dump_queue.size(); i++) { +            if (sid == _dump_queue[i].first) { +                b = _dump_queue[i].second; +                _dump_queue.erase(_dump_queue.begin() + i); +                break; +            } +        } +        return b; +    } + +private: + +    void task_loop(const task_fcn_type &task_fcn){ +        _running = true; +        _spawn_barrier.wait(); + +        try{ +            while (_running){ +            	boost::optional<msg_type_t> buff = task_fcn(); +            	if(buff != boost::none){ +            	    /* +            	     * If a message gets stranded it is returned by task_fcn and then pushed to the dump_queue. +            	     * This way ctrl_cores can check dump_queue for missing messages. +            	     */ +            	    boost::mutex::scoped_lock lock(_mutex); +            	    _dump_queue.push_back(buff.get() ); +            	} +            } +        } +        catch(const boost::thread_interrupted &){ +            //this is an ok way to exit the task loop +        } +        catch(const std::exception &e){ +            do_error_msg(e.what()); +        } +        catch(...){ +            //FIXME +            //Unfortunately, this is also an ok way to end a task, +            //because on some systems boost throws uncatchables. +        } +    } + +    void do_error_msg(const std::string &msg){ +        UHD_MSG(error) +            << "An unexpected exception was caught in a task loop." << std::endl +            << "The task loop will now exit, things may not work." << std::endl +            << msg << std::endl +        ; +    } + +    boost::mutex _mutex; +    boost::thread_group _thread_group; +    boost::barrier _spawn_barrier; +    bool _running; + +    /* +     * This queue holds stranded messages until a radio_ctrl_core grabs them via 'get_msg_from_dump_queue'. +     */ +    std::vector <msg_type_t> _dump_queue; +}; + +msg_task::sptr msg_task::make(const task_fcn_type &task_fcn){ +    return msg_task::sptr(new msg_task_impl(task_fcn)); +} | 
