Send the number of samples per datagram over the control.

Worked on the io impl for usrp2 (added loop unrolls and 32 bit buffers).
Added some vrt rx constants to the fw common used by host and fw.
Removed the MTU prop and added a general device prop for num samples.

6 files changed, 128 insertions, 83 deletions

diff --git a/host/lib/usrp/usrp2/dsp_impl.cpp b/host/lib/usrp/usrp2/dsp_impl.cpp
index e5c4a4245..a57f5ff2d 100644
--- a/host/lib/usrp/usrp2/dsp_impl.cpp
+++ b/host/lib/usrp/usrp2/dsp_impl.cpp
@@ -48,7 +48,7 @@ void usrp2_impl::init_ddc_config(void){
     );
 
     //initial config and update
-    _ddc_decim = 16;
+    _ddc_decim = 64;
     _ddc_freq = 0;
     update_ddc_config();
 
@@ -82,6 +82,12 @@ void usrp2_impl::update_ddc_enabled(void){
     out_data.data.streaming.enabled = (_ddc_enabled)? 1 : 0;
     out_data.data.streaming.secs =  htonl(_ddc_stream_at.secs);
     out_data.data.streaming.ticks = htonl(_ddc_stream_at.ticks);
+    out_data.data.streaming.samples = htonl(
+        _mtu/sizeof(uint32_t) -
+        USRP2_HOST_RX_VRT_HEADER_WORDS32 -
+        USRP2_HOST_RX_VRT_TRAILER_WORDS32 -
+        ((2 + 14 + 20 + 8)/sizeof(uint32_t)) //size of headers (pad, eth, ip, udp)
+    );
 
     //send and recv
     usrp2_ctrl_data_t in_data = ctrl_send_and_recv(out_data);
@@ -151,8 +157,7 @@ void usrp2_impl::ddc_set(const wax::obj &key, const wax::obj &val){
     if (key_name == "decim"){
         size_t new_decim = wax::cast<size_t>(val);
         ASSERT_THROW(std::has(
-            _allowed_decim_and_interp_rates.begin(),
-            _allowed_decim_and_interp_rates.end(),
+            _allowed_decim_and_interp_rates,
             new_decim
         ));
         _ddc_decim = new_decim; //shadow
@@ -196,7 +201,7 @@ void usrp2_impl::init_duc_config(void){
     );
 
     //initial config and update
-    _duc_interp = 16;
+    _duc_interp = 64;
     _duc_freq = 0;
     update_duc_config();
 }
@@ -280,8 +285,7 @@ void usrp2_impl::duc_set(const wax::obj &key, const wax::obj &val){
     if (key_name == "interp"){
         size_t new_interp = wax::cast<size_t>(val);
         ASSERT_THROW(std::has(
-            _allowed_decim_and_interp_rates.begin(),
-            _allowed_decim_and_interp_rates.end(),
+            _allowed_decim_and_interp_rates,
             new_interp
         ));
         _duc_interp = new_interp; //shadow
diff --git a/host/lib/usrp/usrp2/fw_common.h b/host/lib/usrp/usrp2/fw_common.h
index aca0abb28..8e4b2ba35 100644
--- a/host/lib/usrp/usrp2/fw_common.h
+++ b/host/lib/usrp/usrp2/fw_common.h
@@ -27,6 +27,10 @@
 extern "C" {
 #endif
 
+// size of the vrt header and trailer to the host
+#define USRP2_HOST_RX_VRT_HEADER_WORDS32 5
+#define USRP2_HOST_RX_VRT_TRAILER_WORDS32 1 //FIXME fpga sets wrong header size when no trailer present
+
 // udp ports for the usrp2 communication
 // Dynamic and/or private ports: 49152-65535
 #define USRP2_UDP_CTRL_PORT 49152
@@ -175,6 +179,7 @@ typedef struct{
             uint8_t _pad[3];
             uint32_t secs;
             uint32_t ticks;
+            uint32_t samples;
         } streaming;
         struct {
             uint32_t freq_word;
diff --git a/host/lib/usrp/usrp2/io_impl.cpp b/host/lib/usrp/usrp2/io_impl.cpp
index 9c7a41e49..8b45a708a 100644
--- a/host/lib/usrp/usrp2/io_impl.cpp
+++ b/host/lib/usrp/usrp2/io_impl.cpp
@@ -16,7 +16,6 @@
 //
 
 #include <complex>
-#include <boost/shared_array.hpp>
 #include <boost/format.hpp>
 #include "usrp2_impl.hpp"
 
@@ -27,55 +26,80 @@ namespace asio = boost::asio;
 /***********************************************************************
  * Constants
  **********************************************************************/
-typedef std::complex<float> fc32_t;
-typedef std::complex<short> sc16_t;
+typedef std::complex<float>   fc32_t;
+typedef std::complex<int16_t> sc16_t;
 
-static const float float_scale_factor = pow(2.0, 15);
-
-//max length with header, stream id, seconds, fractional seconds
-static const size_t max_vrt_header_words = 5;
+static const float shorts_per_float = pow(2.0, 15);
+static const float floats_per_short = 1.0/shorts_per_float;
 
 /***********************************************************************
  * Helper Functions
  **********************************************************************/
-static inline void host_floats_to_usrp2_shorts(
-    short *usrp2_shorts,
-    const float *host_floats,
+void usrp2_impl::io_init(void){
+    //initially empty spillover buffer
+    _splillover_buff = asio::buffer(_spillover_mem, 0);
+
+    //send a small data packet so the usrp2 knows the udp source port
+    uint32_t zero_data = 0;
+    _data_transport->send(boost::asio::buffer(&zero_data, sizeof(zero_data)));
+}
+
+#define unrolled_loop(__i, __len, __inst) {\
+    size_t __i = 0; \
+    while(__i < (__len & ~0x7)){ \
+        __inst; __i++; __inst; __i++; \
+        __inst; __i++; __inst; __i++; \
+        __inst; __i++; __inst; __i++; \
+        __inst; __i++; __inst; __i++; \
+    } \
+    while(__i < __len){ \
+        __inst; __i++;\
+    } \
+}
+
+static inline void host_floats_to_usrp2_items(
+    uint32_t *usrp2_items,
+    const fc32_t *host_floats,
     size_t num_samps
 ){
-    for(size_t i = 0; i < num_samps; i++){
-        usrp2_shorts[i] = htons(short(host_floats[i]*float_scale_factor));
-    }
+    unrolled_loop(i, num_samps,{
+        int16_t real = host_floats[i].real()*shorts_per_float;
+        int16_t imag = host_floats[i].imag()*shorts_per_float;
+        usrp2_items[i] = htonl(((real << 16) & 0xffff) | ((imag << 0) & 0xffff));
+    });
 }
 
-static inline void usrp2_shorts_to_host_floats(
-    float *host_floats,
-    const short *usrp2_shorts,
+static inline void usrp2_items_to_host_floats(
+    fc32_t *host_floats,
+    const uint32_t *usrp2_items,
     size_t num_samps
 ){
-    for(size_t i = 0; i < num_samps; i++){
-        host_floats[i] = float(short(ntohs(usrp2_shorts[i])))/float_scale_factor;
-    }
+    unrolled_loop(i, num_samps,{
+        uint32_t item = ntohl(usrp2_items[i]);
+        int16_t real = (item >> 16) & 0xffff;
+        int16_t imag = (item >> 0)  & 0xffff;
+        host_floats[i] = fc32_t(real*floats_per_short, imag*floats_per_short);
+    });
 }
 
-static inline void host_shorts_to_usrp2_shorts(
-    short *usrp2_shorts,
-    const short *host_shorts,
+static inline void host_items_to_usrp2_items(
+    uint32_t *usrp2_items,
+    const uint32_t *host_items,
     size_t num_samps
 ){
-    for(size_t i = 0; i < num_samps; i++){
-        usrp2_shorts[i] = htons(host_shorts[i]);
-    }
+    unrolled_loop(i, num_samps,
+        usrp2_items[i] = htonl(host_items[i])
+    );
 }
 
-static inline void usrp2_shorts_to_host_shorts(
-    short *host_shorts,
-    const short *usrp2_shorts,
+static inline void usrp2_items_to_host_items(
+    uint32_t *host_items,
+    const uint32_t *usrp2_items,
     size_t num_samps
 ){
-    for(size_t i = 0; i < num_samps; i++){
-        host_shorts[i] = ntohs(usrp2_shorts[i]);
-    }
+    unrolled_loop(i, num_samps,
+        host_items[i] = ntohl(usrp2_items[i])
+    );
 }
 
 /***********************************************************************
@@ -86,7 +110,7 @@ size_t usrp2_impl::send_raw(
     const uhd::metadata_t &metadata
 ){
     std::vector<boost::asio::const_buffer> buffs(2);
-    uint32_t vrt_hdr[max_vrt_header_words];
+    uint32_t vrt_hdr[7]; //max size
     uint32_t vrt_hdr_flags = 0;
     size_t num_vrt_hdr_words = 1;
 
@@ -107,7 +131,7 @@ size_t usrp2_impl::send_raw(
 
     //fill in complete header word
     vrt_hdr[0] = htonl(vrt_hdr_flags |
-        ((_stream_id_to_packet_seq[metadata.stream_id]++ & 0xf) << 16) |
+        ((_tx_stream_id_to_packet_seq[metadata.stream_id]++ & 0xf) << 16) |
         (num_vrt_hdr_words & 0xffff)
     );
 
@@ -148,18 +172,16 @@ size_t usrp2_impl::recv_raw(
 
     //load the buffer vector
     std::vector<boost::asio::mutable_buffer> buffs(3);
-    uint32_t vrt_hdr[max_vrt_header_words];
-    buffs[0] = asio::buffer(vrt_hdr, max_vrt_header_words*sizeof(uint32_t));
-    buffs[1] = asio::buffer(//make sure its on a word boundary
-        buff, asio::buffer_size(buff) & ~(sizeof(uint32_t) - 1)
-    );
+    uint32_t vrt_hdr[USRP2_HOST_RX_VRT_HEADER_WORDS32];
+    buffs[0] = asio::buffer(vrt_hdr, sizeof(vrt_hdr));
+    buffs[1] = buff;
     buffs[2] = asio::buffer(_spillover_mem, _mtu);
 
     //receive into the buffers
     size_t bytes_recvd = _data_transport->recv(buffs);
 
     //failure case
-    if (bytes_recvd < max_vrt_header_words*sizeof(uint32_t)) return 0;
+    if (bytes_recvd < sizeof(vrt_hdr)) return 0;
 
     //unpack the vrt header
     metadata = uhd::metadata_t();
@@ -170,8 +192,15 @@ size_t usrp2_impl::recv_raw(
     metadata.time_spec.secs = ntohl(vrt_hdr[2]);
     metadata.time_spec.ticks = ntohl(vrt_hdr[3]);
 
+    size_t my_seq = (vrt_header >> 16) & 0xf;
+    //std::cout << "seq " << my_seq << std::endl;
+    if (my_seq != ((_rx_stream_id_to_packet_seq[metadata.stream_id]+1) & 0xf)) std::cout << "bad seq " << my_seq << std::endl;
+    _rx_stream_id_to_packet_seq[metadata.stream_id] = my_seq;
+
     //extract the number of bytes received
-    size_t num_words = (vrt_header & 0xffff) - max_vrt_header_words;
+    size_t num_words = (vrt_header & 0xffff);
+    num_words -= USRP2_HOST_RX_VRT_HEADER_WORDS32;
+    num_words -= USRP2_HOST_RX_VRT_TRAILER_WORDS32;
     size_t num_bytes = num_words*sizeof(uint32_t);
 
     //handle the case where spillover memory was used
@@ -191,13 +220,12 @@ size_t usrp2_impl::send(
 ){
     if (type == "32fc"){
         size_t num_samps = asio::buffer_size(buff)/sizeof(fc32_t);
-        boost::shared_array<sc16_t> raw_mem(new sc16_t[num_samps]);
-        boost::asio::mutable_buffer raw_buff(raw_mem.get(), num_samps*sizeof(sc16_t));
+        boost::asio::mutable_buffer raw_buff(_tmp_send_mem, num_samps*sizeof(sc16_t));
 
-        host_floats_to_usrp2_shorts(
-            asio::buffer_cast<short*>(raw_buff),
-            asio::buffer_cast<const float*>(buff),
-            num_samps*2 //double for complex
+        host_floats_to_usrp2_items(
+            asio::buffer_cast<uint32_t*>(raw_buff),
+            asio::buffer_cast<const fc32_t*>(buff),
+            num_samps
         );
 
         return send_raw(raw_buff, metadata);
@@ -208,13 +236,12 @@ size_t usrp2_impl::send(
         return send_raw(buff, metadata);
         #else
         size_t num_samps = asio::buffer_size(buff)/sizeof(sc16_t);
-        boost::shared_array<sc16_t> raw_mem(new sc16_t[num_samps]);
-        boost::asio::mutable_buffer raw_buff(raw_mem.get(), num_samps*sizeof(sc16_t));
+        boost::asio::mutable_buffer raw_buff(_tmp_send_mem, num_samps*sizeof(sc16_t));
 
-        host_shorts_to_usrp2_shorts(
-            asio::buffer_cast<short*>(raw_buff),
-            asio::buffer_cast<const short*>(buff),
-            num_samps*2 //double for complex
+        host_items_to_usrp2_items(
+            asio::buffer_cast<uint32_t*>(raw_buff),
+            asio::buffer_cast<const uint32_t*>(buff),
+            num_samps
         );
 
         return send_raw(raw_buff, metadata);
@@ -234,15 +261,14 @@ size_t usrp2_impl::recv(
 ){
     if (type == "32fc"){
         size_t num_samps = asio::buffer_size(buff)/sizeof(fc32_t);
-        boost::shared_array<sc16_t> raw_mem(new sc16_t[num_samps]);
-        boost::asio::mutable_buffer raw_buff(raw_mem.get(), num_samps*sizeof(sc16_t));
+        boost::asio::mutable_buffer raw_buff(_tmp_recv_mem, num_samps*sizeof(sc16_t));
 
         num_samps = recv_raw(raw_buff, metadata);
 
-        usrp2_shorts_to_host_floats(
-            asio::buffer_cast<float*>(buff),
-            asio::buffer_cast<const short*>(raw_buff),
-            num_samps*2 //double for complex
+        usrp2_items_to_host_floats(
+            asio::buffer_cast<fc32_t*>(buff),
+            asio::buffer_cast<const uint32_t*>(raw_buff),
+            num_samps
         );
 
         return num_samps;
@@ -253,15 +279,14 @@ size_t usrp2_impl::recv(
         return recv_raw(buff, metadata);
         #else
         size_t num_samps = asio::buffer_size(buff)/sizeof(sc16_t);
-        boost::shared_array<sc16_t> raw_mem(new sc16_t[num_samps]);
-        boost::asio::mutable_buffer raw_buff(raw_mem.get(), num_samps*sizeof(sc16_t));
+        boost::asio::mutable_buffer raw_buff(_tmp_recv_mem, num_samps*sizeof(sc16_t));
 
         num_samps = recv_raw(raw_buff, metadata);
 
-        usrp2_shorts_to_host_shorts(
-            asio::buffer_cast<short*>(buff),
-            asio::buffer_cast<const short*>(raw_buff),
-            num_samps*2 //double for complex
+        usrp2_items_to_host_items(
+            asio::buffer_cast<uint32_t*>(buff),
+            asio::buffer_cast<const uint32_t*>(raw_buff),
+            num_samps
         );
 
         return num_samps;
diff --git a/host/lib/usrp/usrp2/mboard_impl.cpp b/host/lib/usrp/usrp2/mboard_impl.cpp
index cc73b229c..8e682a675 100644
--- a/host/lib/usrp/usrp2/mboard_impl.cpp
+++ b/host/lib/usrp/usrp2/mboard_impl.cpp
@@ -82,6 +82,7 @@ void usrp2_impl::mboard_get(const wax::obj &key_, wax::obj &val){
         return;
 
     case MBOARD_PROP_RX_DBOARD:
+        ASSERT_THROW(_rx_dboards.has_key(name));
         val = _rx_dboards[name].get_link();
         return;
 
@@ -90,6 +91,7 @@ void usrp2_impl::mboard_get(const wax::obj &key_, wax::obj &val){
         return;
 
     case MBOARD_PROP_TX_DBOARD:
+        ASSERT_THROW(_tx_dboards.has_key(name));
         val = _tx_dboards[name].get_link();
         return;
 
@@ -97,17 +99,12 @@ void usrp2_impl::mboard_get(const wax::obj &key_, wax::obj &val){
         val = prop_names_t(_tx_dboards.get_keys());
         return;
 
-    case MBOARD_PROP_MTU:
-        // FIXME we dont know the real MTU...
-        // give them something to fragment about
-        val = size_t(1500);
-        return;
-
     case MBOARD_PROP_CLOCK_RATE:
         val = freq_t(get_master_clock_freq());
         return;
 
     case MBOARD_PROP_RX_DSP:
+        ASSERT_THROW(_rx_dsps.has_key(name));
         val = _rx_dsps[name].get_link();
         return;
 
@@ -116,6 +113,7 @@ void usrp2_impl::mboard_get(const wax::obj &key_, wax::obj &val){
         return;
 
     case MBOARD_PROP_TX_DSP:
+        ASSERT_THROW(_tx_dsps.has_key(name));
         val = _tx_dsps[name].get_link();
         return;
 
@@ -183,7 +181,6 @@ void usrp2_impl::mboard_set(const wax::obj &key, const wax::obj &val){
 
     case MBOARD_PROP_NAME:
     case MBOARD_PROP_OTHERS:
-    case MBOARD_PROP_MTU:
     case MBOARD_PROP_CLOCK_RATE:
     case MBOARD_PROP_RX_DSP:
     case MBOARD_PROP_RX_DSP_NAMES:
diff --git a/host/lib/usrp/usrp2/usrp2_impl.cpp b/host/lib/usrp/usrp2/usrp2_impl.cpp
index 06876d241..700f94ae1 100644
--- a/host/lib/usrp/usrp2/usrp2_impl.cpp
+++ b/host/lib/usrp/usrp2/usrp2_impl.cpp
@@ -133,9 +133,8 @@ usrp2_impl::usrp2_impl(
     //init the tx and rx dboards (do last)
     dboard_init();
 
-    //send a small data packet so the usrp2 knows the udp source port
-    uint32_t zero_data = 0;
-    _data_transport->send(boost::asio::buffer(&zero_data, sizeof(zero_data)));
+    //init the send and recv io
+    io_init();
 
 }
 
@@ -197,12 +196,22 @@ void usrp2_impl::get(const wax::obj &key_, wax::obj &val){
         return;
 
     case DEVICE_PROP_MBOARD:
+        ASSERT_THROW(_mboards.has_key(name));
         val = _mboards[name].get_link();
         return;
 
     case DEVICE_PROP_MBOARD_NAMES:
         val = prop_names_t(_mboards.get_keys());
         return;
+
+    case DEVICE_PROP_MAX_RX_SAMPLES:
+        val = size_t(_max_samples_per_packet);
+        return;
+
+    case DEVICE_PROP_MAX_TX_SAMPLES:
+        val = size_t(_max_samples_per_packet);
+        return;
+
     }
 }
 
diff --git a/host/lib/usrp/usrp2/usrp2_impl.hpp b/host/lib/usrp/usrp2/usrp2_impl.hpp
index 47b01d1b1..43f32c6e6 100644
--- a/host/lib/usrp/usrp2/usrp2_impl.hpp
+++ b/host/lib/usrp/usrp2/usrp2_impl.hpp
@@ -105,10 +105,15 @@ private:
     //the raw io interface (samples are in the usrp2 native format)
     size_t send_raw(const boost::asio::const_buffer &, const uhd::metadata_t &);
     size_t recv_raw(const boost::asio::mutable_buffer &, uhd::metadata_t &);
-    uhd::dict<uint32_t, size_t> _stream_id_to_packet_seq;
-    static const size_t _mtu = 1500;
-    uint8_t _spillover_mem[_mtu];
+    uhd::dict<uint32_t, size_t> _tx_stream_id_to_packet_seq;
+    uhd::dict<uint32_t, size_t> _rx_stream_id_to_packet_seq;
+    static const size_t _mtu = 1500; //FIXME we have no idea
+    static const size_t _max_samples_per_packet = _mtu/sizeof(uint32_t);
+    uint32_t _tmp_send_mem[_mtu/sizeof(uint32_t)];
+    uint32_t _tmp_recv_mem[_mtu/sizeof(uint32_t)];
+    uint32_t _spillover_mem[_mtu/sizeof(uint32_t)];
     boost::asio::mutable_buffer _splillover_buff;
+    void io_init(void);
 
     //udp transports for control and data
     uhd::transport::udp::sptr _ctrl_transport;