From 451067295399e357d73c9bfdeef5f2ad040e0243 Mon Sep 17 00:00:00 2001 From: Josh Blum Date: Mon, 1 Mar 2010 16:13:30 -0800 Subject: 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. --- host/lib/usrp/usrp2/io_impl.cpp | 153 +++++++++++++++++++++++----------------- 1 file changed, 89 insertions(+), 64 deletions(-) (limited to 'host/lib/usrp/usrp2/io_impl.cpp') 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 -#include #include #include "usrp2_impl.hpp" @@ -27,55 +26,80 @@ namespace asio = boost::asio; /*********************************************************************** * Constants **********************************************************************/ -typedef std::complex fc32_t; -typedef std::complex sc16_t; +typedef std::complex fc32_t; +typedef std::complex 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 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 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 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(raw_buff), - asio::buffer_cast(buff), - num_samps*2 //double for complex + host_floats_to_usrp2_items( + asio::buffer_cast(raw_buff), + asio::buffer_cast(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 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(raw_buff), - asio::buffer_cast(buff), - num_samps*2 //double for complex + host_items_to_usrp2_items( + asio::buffer_cast(raw_buff), + asio::buffer_cast(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 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(buff), - asio::buffer_cast(raw_buff), - num_samps*2 //double for complex + usrp2_items_to_host_floats( + asio::buffer_cast(buff), + asio::buffer_cast(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 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(buff), - asio::buffer_cast(raw_buff), - num_samps*2 //double for complex + usrp2_items_to_host_items( + asio::buffer_cast(buff), + asio::buffer_cast(raw_buff), + num_samps ); return num_samps; -- cgit v1.2.3