b100: support for dual dsp

1 files changed, 50 insertions, 49 deletions

diff --git a/host/lib/usrp/b100/io_impl.cpp b/host/lib/usrp/b100/io_impl.cpp
index 3978bea75..5377c43d5 100644
--- a/host/lib/usrp/b100/io_impl.cpp
+++ b/host/lib/usrp/b100/io_impl.cpp
@@ -1,5 +1,5 @@
 //
-// Copyright 2010 Ettus Research LLC
+// Copyright 2011 Ettus Research LLC
 //
 // This program is free software: you can redistribute it and/or modify
 // it under the terms of the GNU General Public License as published by
@@ -45,17 +45,45 @@ struct b100_impl::io_impl{
     io_impl(zero_copy_if::sptr data_transport):
         data_transport(data_transport)
     {
-        /* NOP */
+        for (size_t i = 0; i < B100_NUM_RX_DSPS; i++){
+            typedef bounded_buffer<managed_recv_buffer::sptr> buffs_queue_type;
+            _buffs_queue.push_back(new buffs_queue_type(data_transport->get_num_recv_frames()));
+        }
     }
 
     ~io_impl(void){
-        //drain the rx buffs
-        //while(data_transport->get_recv_buff().get() != NULL){
-                /* NOP */
-        //}
+        for (size_t i = 0; i < _buffs_queue.size(); i++){
+            delete _buffs_queue[i];
+        }
     }
 
-    zero_copy_if::sptr &data_transport;
+    zero_copy_if::sptr data_transport;
+
+    std::vector<bounded_buffer<managed_recv_buffer::sptr> *> _buffs_queue;
+
+    //gets buffer, determines if its the requested index,
+    //and either queues the buffer or returns the buffer
+    managed_recv_buffer::sptr get_recv_buff(const size_t index, const double timeout){
+        while (true){
+            managed_recv_buffer::sptr buff;
+
+            //attempt to pop a buffer from the queue
+            if (_buffs_queue[index]->pop_with_haste(buff)) return buff;
+
+            //otherwise, call into the transport
+            buff = data_transport->get_recv_buff(timeout);
+            if (buff.get() == NULL) return buff; //timeout
+
+            //check the stream id to know which channel
+            const boost::uint32_t *vrt_hdr = buff->cast<const boost::uint32_t *>();
+            const size_t rx_index = uhd::wtohx(vrt_hdr[1]) - B100_DSP_SID_BASE;
+            if (rx_index == index) return buff; //got expected message
+
+            //otherwise queue and try again
+            if (rx_index < B100_NUM_RX_DSPS) _buffs_queue[rx_index]->push_with_pop_on_full(buff);
+            else UHD_MSG(error) << "Got a data packet with known SID " << uhd::wtohx(vrt_hdr[1]) << std::endl;
+        }
+    }
 
     sph::recv_packet_handler recv_handler;
     sph::send_packet_handler send_handler;
@@ -66,46 +94,17 @@ struct b100_impl::io_impl{
  * Initialize internals within this file
  **********************************************************************/
 void b100_impl::io_init(void){
-    _recv_otw_type.width = 16;
-    _recv_otw_type.shift = 0;
-    _recv_otw_type.byteorder = otw_type_t::BO_LITTLE_ENDIAN;
-
-    _send_otw_type.width = 16;
-    _send_otw_type.shift = 0;
-    _send_otw_type.byteorder = otw_type_t::BO_LITTLE_ENDIAN;
-    
     _iface->reset_gpif(6);
 
-    //reset state machines
-    _iface->poke32(B100_REG_CTRL_TX_CLEAR_UNDERRUN, 0);
-    _iface->poke32(B100_REG_CTRL_RX_CLEAR_OVERRUN, 0);
-
     _io_impl = UHD_PIMPL_MAKE(io_impl, (_data_transport));
-    
-    //setup rx data path
-    _iface->poke32(B100_REG_CTRL_RX_NSAMPS_PER_PKT, get_max_recv_samps_per_packet());
-    UHD_LOGV(always) << "IO: Using " << get_max_recv_samps_per_packet() << " samples per packet" << std::endl;
-    _iface->poke32(B100_REG_CTRL_RX_NCHANNELS, 1);
-    _iface->poke32(B100_REG_CTRL_RX_VRT_HEADER, 0
-        | (0x1 << 28) //if data with stream id
-        | (0x1 << 26) //has trailer
-        | (0x3 << 22) //integer time other
-        | (0x1 << 20) //fractional time sample count
-    );
-    _iface->poke32(B100_REG_CTRL_RX_VRT_TRAILER, 0);
 
-    //set the streamid to reset the seq num
-    _iface->poke32(B100_REG_CTRL_TX_REPORT_SID, 0);
-    //setup the tx policy
-    _iface->poke32(B100_REG_CTRL_TX_POLICY, B100_FLAG_CTRL_TX_POLICY_NEXT_PACKET);
-    
     //set the expected packet size in USB frames
     _iface->poke32(B100_REG_MISC_RX_LEN, 4);
 
-    update_transport_channel_mapping();
+    update_xport_channel_mapping();
 }
 
-void b100_impl::update_transport_channel_mapping(void){
+void b100_impl::update_xport_channel_mapping(void){
     if (_io_impl.get() == NULL) return; //not inited yet
 
     //set all of the relevant properties on the handler
@@ -113,10 +112,11 @@ void b100_impl::update_transport_channel_mapping(void){
     _io_impl->recv_handler.resize(_rx_subdev_spec.size());
     _io_impl->recv_handler.set_vrt_unpacker(&vrt::if_hdr_unpack_le);
     _io_impl->recv_handler.set_tick_rate(_clock_ctrl->get_fpga_clock_rate());
-    _io_impl->recv_handler.set_samp_rate(_rx_ddc_proxy->get_link()[DSP_PROP_HOST_RATE].as<double>());
+    //FIXME assumes homogeneous rates across all dsp
+    _io_impl->recv_handler.set_samp_rate(_rx_dsp_proxies[_rx_dsp_proxies.keys().at(0)]->get_link()[DSP_PROP_HOST_RATE].as<double>());
     for (size_t chan = 0; chan < _io_impl->recv_handler.size(); chan++){
         _io_impl->recv_handler.set_xport_chan_get_buff(chan, boost::bind(
-            &uhd::transport::zero_copy_if::get_recv_buff, _io_impl->data_transport, _1
+            &b100_impl::io_impl::get_recv_buff, _io_impl.get(), chan, _1
         ));
         _io_impl->recv_handler.set_overflow_handler(chan, boost::bind(
             &b100_impl::handle_overrun, this, chan
@@ -129,7 +129,8 @@ void b100_impl::update_transport_channel_mapping(void){
     _io_impl->send_handler.resize(_tx_subdev_spec.size());
     _io_impl->send_handler.set_vrt_packer(&vrt::if_hdr_pack_le);
     _io_impl->send_handler.set_tick_rate(_clock_ctrl->get_fpga_clock_rate());
-    _io_impl->send_handler.set_samp_rate(_tx_duc_proxy->get_link()[DSP_PROP_HOST_RATE].as<double>());
+    //FIXME assumes homogeneous rates across all dsp
+    _io_impl->send_handler.set_samp_rate(_tx_dsp_proxies[_tx_dsp_proxies.keys().at(0)]->get_link()[DSP_PROP_HOST_RATE].as<double>());
     for (size_t chan = 0; chan < _io_impl->send_handler.size(); chan++){
         _io_impl->send_handler.set_xport_chan_get_buff(chan, boost::bind(
             &uhd::transport::zero_copy_if::get_send_buff, _io_impl->data_transport, _1
@@ -155,7 +156,7 @@ size_t b100_impl::send(
     const send_buffs_type &buffs, size_t nsamps_per_buff,
     const tx_metadata_t &metadata, const io_type_t &io_type,
     send_mode_t send_mode, double timeout
-){    
+){
     return _io_impl->send_handler.send(
         buffs, nsamps_per_buff,
         metadata, io_type,
@@ -189,13 +190,13 @@ size_t b100_impl::recv(
     );
 }
 
-void b100_impl::issue_stream_cmd(const stream_cmd_t &stream_cmd)
+void b100_impl::issue_ddc_stream_cmd(const stream_cmd_t &stream_cmd, size_t index)
 {
     _io_impl->continuous_streaming = (stream_cmd.stream_mode == stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
-    _iface->poke32(B100_REG_CTRL_RX_STREAM_CMD, dsp_type1::calc_stream_cmd_word(stream_cmd));
-    _iface->poke32(B100_REG_CTRL_RX_TIME_SECS,  boost::uint32_t(stream_cmd.time_spec.get_full_secs()));
-    _iface->poke32(B100_REG_CTRL_RX_TIME_TICKS, stream_cmd.time_spec.get_tick_count(_clock_ctrl->get_fpga_clock_rate()));
-    
+    _iface->poke32(B100_REG_RX_CTRL_STREAM_CMD(index), dsp_type1::calc_stream_cmd_word(stream_cmd));
+    _iface->poke32(B100_REG_RX_CTRL_TIME_SECS(index),  boost::uint32_t(stream_cmd.time_spec.get_full_secs()));
+    _iface->poke32(B100_REG_RX_CTRL_TIME_TICKS(index), stream_cmd.time_spec.get_tick_count(_clock_ctrl->get_fpga_clock_rate()));
+
     if (stream_cmd.stream_mode == stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS) {
         while(_io_impl->data_transport->get_recv_buff().get() != NULL){
             /* NOP */
@@ -203,8 +204,8 @@ void b100_impl::issue_stream_cmd(const stream_cmd_t &stream_cmd)
     }
 }
 
-void b100_impl::handle_overrun(size_t){
+void b100_impl::handle_overrun(size_t index){
     if (_io_impl->continuous_streaming){
-        this->issue_stream_cmd(stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
+        this->issue_ddc_stream_cmd(stream_cmd_t::STREAM_MODE_START_CONTINUOUS, index);
     }
 }