From 8050fda48d69f46788672a9ceaccd8d82500ac05 Mon Sep 17 00:00:00 2001 From: Josh Blum Date: Tue, 23 Feb 2010 16:27:49 -0800 Subject: Added IF data io handing within the usrp2 impl. It packs and unpacks vrt headers/metadata. NOT YET TESTED IN ANY WAY... --- host/lib/usrp/usrp2/io_impl.cpp | 261 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 261 insertions(+) create mode 100644 host/lib/usrp/usrp2/io_impl.cpp (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 new file mode 100644 index 000000000..fbea71f85 --- /dev/null +++ b/host/lib/usrp/usrp2/io_impl.cpp @@ -0,0 +1,261 @@ +// +// Copyright 2010 Ettus Research LLC +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . +// + +#include +#include +#include +#include "usrp2_impl.hpp" + +using namespace uhd; +using namespace uhd::usrp; + +/*********************************************************************** + * Constants + **********************************************************************/ +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; + +/*********************************************************************** + * Helper Functions + **********************************************************************/ + +/*static void pack_vrt_header( + size_t num_data_words, //input + const uhd::metadata_t &metadata, //input + uint32_t *vrt_header, //output + size_t &vrt_header_words //output +){ + // +} + +static void unpack_vrt_header( + size_t &num_data_words, //output + uhd::metadata_t &metadata, //output + const uint32_t *vrt_header, //input + size_t &vrt_header_words //output +){ + // +}*/ + +static inline void host_floats_to_usrp2_shorts( + int16_t *usrp2_shorts, + const float *host_floats, + size_t num_samps +){ + for(size_t i = 0; i < num_samps; i++){ + usrp2_shorts[i] = htons(int16_t(host_floats[i]*float_scale_factor)); + } +} + +static inline void usrp2_shorts_to_host_floats( + float *host_floats, + const int16_t *usrp2_shorts, + size_t num_samps +){ + for(size_t i = 0; i < num_samps; i++){ + host_floats[i] = float(ntohs(usrp2_shorts[i])/float_scale_factor); + } +} + +static inline void host_shorts_to_usrp2_shorts( + int16_t *usrp2_shorts, + const int16_t *host_shorts, + size_t num_samps +){ + for(size_t i = 0; i < num_samps; i++){ + usrp2_shorts[i] = htons(host_shorts[i]); + } +} + +static inline void usrp2_shorts_to_host_shorts( + int16_t *host_shorts, + const int16_t *usrp2_shorts, + size_t num_samps +){ + for(size_t i = 0; i < num_samps; i++){ + host_shorts[i] = ntohs(usrp2_shorts[i]); + } +} + +/*********************************************************************** + * Send Raw Data + **********************************************************************/ +size_t usrp2_impl::send_raw( + const boost::asio::const_buffer &buff, + const uhd::metadata_t &metadata +){ + std::vector buffs(2); + uint32_t vrt_hdr[max_vrt_header_words]; + uint32_t vrt_hdr_flags = 0; + size_t num_vrt_hdr_words = 1; + + //load the vrt header and flags + if(metadata.has_stream_id){ + vrt_hdr_flags |= (0x1 << 28); //IF Data packet with Stream Identifier + vrt_hdr[num_vrt_hdr_words++] = htonl(metadata.stream_id); + } + if(metadata.has_time_spec){ + vrt_hdr_flags |= (0x3 << 22) | (0x1 << 20); //TSI: Other, TSF: Sample Count Timestamp + vrt_hdr[num_vrt_hdr_words++] = htonl(metadata.time_spec.secs); + vrt_hdr[num_vrt_hdr_words++] = htonl(metadata.time_spec.ticks); + vrt_hdr[num_vrt_hdr_words++] = 0; //unused part of fractional seconds + } + vrt_hdr_flags |= (metadata.start_of_burst)? (0x1 << 25) : 0; + vrt_hdr_flags |= (metadata.end_of_burst)? (0x1 << 24) : 0; + + //fill in complete header word + vrt_hdr[0] = htonl(vrt_hdr_flags | + ((_stream_id_to_packet_seq[metadata.stream_id]++ << 16) & 0xf) | + ((boost::asio::buffer_size(buff)/sizeof(uint32_t)) & 0xffff) + ); + + //load the buffer vector + size_t vrt_hdr_size = num_vrt_hdr_words*sizeof(uint32_t); + buffs[0] = boost::asio::buffer(&vrt_hdr, vrt_hdr_size); + buffs[1] = buff; + + //send and return number of samples + return (_data_transport->send(buffs) - vrt_hdr_size)/sizeof(sc16_t); +} + +/*********************************************************************** + * Receive Raw Data + **********************************************************************/ +size_t usrp2_impl::recv_raw( + const boost::asio::mutable_buffer &buff, + uhd::metadata_t &metadata +){ + //load the buffer vector + std::vector buffs(2); + uint32_t vrt_hdr[max_vrt_header_words]; + buffs[0] = boost::asio::buffer(vrt_hdr, max_vrt_header_words); + buffs[1] = buff; + + //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; + + //unpack the vrt header + metadata = uhd::metadata_t(); + uint32_t vrt_header = ntohl(vrt_hdr[0]); + metadata.has_stream_id = true; + metadata.stream_id = ntohl(vrt_hdr[1]); + metadata.has_time_spec = true; + metadata.time_spec.secs = ntohl(vrt_hdr[2]); + metadata.time_spec.ticks = ntohl(vrt_hdr[3]); + + //return the number of samples received + size_t num_words = vrt_header & 0xffff; + return (num_words*sizeof(uint32_t))/sizeof(sc16_t); +} + +/*********************************************************************** + * Send Data + **********************************************************************/ +size_t usrp2_impl::send( + const boost::asio::const_buffer &buff, + const uhd::metadata_t &metadata, + const std::string &type +){ + if (type == "fc32"){ + size_t num_samps = boost::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)); + + host_floats_to_usrp2_shorts( + boost::asio::buffer_cast(raw_buff), + boost::asio::buffer_cast(buff), + num_samps*2 //double for complex + ); + + return send_raw(raw_buff, metadata); + } + + if (type == "sc16"){ + #ifdef HAVE_BIG_ENDIAN + return send_raw(buff, metadata); + #else + size_t num_samps = boost::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)); + + host_shorts_to_usrp2_shorts( + boost::asio::buffer_cast(raw_buff), + boost::asio::buffer_cast(buff), + num_samps*2 //double for complex + ); + + return send_raw(raw_buff, metadata); + #endif + } + + throw std::runtime_error(str(boost::format("usrp2 send: cannot handle type \"%s\"") % type)); +} + +/*********************************************************************** + * Receive Data + **********************************************************************/ +size_t usrp2_impl::recv( + const boost::asio::mutable_buffer &buff, + uhd::metadata_t &metadata, + const std::string &type +){ + if (type == "fc32"){ + size_t num_samps = boost::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)); + + num_samps = recv_raw(raw_buff, metadata); + + usrp2_shorts_to_host_floats( + boost::asio::buffer_cast(buff), + boost::asio::buffer_cast(raw_buff), + num_samps*2 //double for complex + ); + + return num_samps; + } + + if (type == "sc16"){ + #ifdef HAVE_BIG_ENDIAN + return recv_raw(buff, metadata); + #else + size_t num_samps = boost::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)); + + num_samps = recv_raw(raw_buff, metadata); + + usrp2_shorts_to_host_shorts( + boost::asio::buffer_cast(buff), + boost::asio::buffer_cast(raw_buff), + num_samps*2 //double for complex + ); + + return num_samps; + #endif + } + + throw std::runtime_error(str(boost::format("usrp2 recv: cannot handle type \"%s\"") % type)); +} -- cgit v1.2.3 From 588278f56766b0349115dec81d3fb714215c3774 Mon Sep 17 00:00:00 2001 From: Josh Blum Date: Tue, 23 Feb 2010 17:58:41 -0800 Subject: Put fast path code (rx setup) back into txrx.c. Store the fast path addrs in the udp data handler. --- firmware/microblaze/apps/txrx.c | 88 +++++++++++++++++++++++++++-------------- host/lib/usrp/usrp2/io_impl.cpp | 19 --------- 2 files changed, 59 insertions(+), 48 deletions(-) (limited to 'host/lib/usrp/usrp2/io_impl.cpp') diff --git a/firmware/microblaze/apps/txrx.c b/firmware/microblaze/apps/txrx.c index 16aa8eab2..1724284b0 100644 --- a/firmware/microblaze/apps/txrx.c +++ b/firmware/microblaze/apps/txrx.c @@ -47,6 +47,8 @@ #include #include #include +#include +#include #define FW_SETS_SEQNO 1 // define to 0 or 1 (FIXME must be 1 for now) @@ -136,10 +138,14 @@ static int streaming_frame_count = 0; bool is_streaming(void){ return streaming_p; } - // ---------------------------------------------------------------- +// the fast-path setup global variables +// ---------------------------------------------------------------- +static eth_mac_addr_t fp_mac_addr_src, fp_mac_addr_dst; +static struct socket_address fp_socket_src, fp_socket_dst; -void start_rx_streaming_cmd(void *p); +// ---------------------------------------------------------------- +void start_rx_streaming_cmd(void); void stop_rx_cmd(void); static eth_mac_addr_t get_my_eth_mac_addr(void){ @@ -158,11 +164,38 @@ void handle_udp_data_packet( struct socket_address src, struct socket_address dst, unsigned char *payload, int payload_len ){ - //TODO store the reply port - //forward this data to the dsp when the payload is sufficient //the small payload is used to give the device the udp source port - _is_data = payload_len > sizeof(uint32_t); + if (payload_len > sizeof(uint32_t)){ + _is_data = true; + return; + } + + //its a tiny payload, load the fast-path variables + fp_mac_addr_src = get_my_eth_mac_addr(); + arp_cache_lookup_mac(&src.addr, &fp_mac_addr_dst); + fp_socket_src = dst; + fp_socket_dst = src; + printf("Storing for fast path:\n"); + printf(" source mac addr: "); + print_mac_addr(fp_mac_addr_src.addr); newline(); + printf(" source ip addr: %d.%d.%d.%d\n", + ((const unsigned char*)&fp_socket_src.addr.addr)[0], + ((const unsigned char*)&fp_socket_src.addr.addr)[1], + ((const unsigned char*)&fp_socket_src.addr.addr)[2], + ((const unsigned char*)&fp_socket_src.addr.addr)[3] + ); + printf(" source udp port: %d\n", fp_socket_src.port); + printf(" destination mac addr: "); + print_mac_addr(fp_mac_addr_dst.addr); newline(); + printf(" destination ip addr: %d.%d.%d.%d\n", + ((const unsigned char*)&fp_socket_dst.addr.addr)[0], + ((const unsigned char*)&fp_socket_dst.addr.addr)[1], + ((const unsigned char*)&fp_socket_dst.addr.addr)[2], + ((const unsigned char*)&fp_socket_dst.addr.addr)[3] + ); + printf(" destination udp port: %d\n", fp_socket_dst.port); + newline(); } #define OTW_GPIO_BANK_TO_NUM(bank) \ @@ -426,7 +459,7 @@ void handle_udp_ctrl_packet( stop_rx_cmd(); } else{ - start_rx_streaming_cmd(NULL); + start_rx_streaming_cmd(); } ctrl_data_out.id = USRP2_CTRL_ID_CONFIGURED_THAT_STREAMING_DUDE; @@ -483,7 +516,7 @@ eth_pkt_inspector(dbsm_t *sm, int bufno) //------------------------------------------------------------------ #define VRT_HEADER_WORDS 5 -#define VRT_TRAILER_WORDS 1 +#define VRT_TRAILER_WORDS 0 void restart_streaming(void) @@ -494,7 +527,7 @@ restart_streaming(void) sr_rx_ctrl->clear_overrun = 1; // reset sr_rx_ctrl->vrt_header = (0 | VRTH_PT_IF_DATA_WITH_SID - | VRTH_HAS_TRAILER + | ((VRT_TRAILER_WORDS)? VRTH_HAS_TRAILER : 0) | VRTH_TSI_OTHER | VRTH_TSF_SAMPLE_CNT | (VRT_HEADER_WORDS+streaming_items_per_frame+VRT_TRAILER_WORDS)); @@ -527,14 +560,14 @@ restart_streaming(void) * * init chksum to zero to start. */ -/*static unsigned int +static unsigned int CHKSUM(unsigned int x, unsigned int *chksum) { *chksum += x; *chksum = (*chksum & 0xffff) + (*chksum>>16); *chksum = (*chksum & 0xffff) + (*chksum>>16); return x; -}*/ +} /* * Called when eth phy state changes (w/ interrupts disabled) @@ -549,7 +582,7 @@ link_changed_callback(int speed) } void -start_rx_streaming_cmd(void *p) +start_rx_streaming_cmd(void) { /* * Construct ethernet header and preload into two buffers @@ -559,31 +592,31 @@ start_rx_streaming_cmd(void *p) } mem _AL4; memset(&mem, 0, sizeof(mem)); - //p->items_per_frame = (1500)/sizeof(uint32_t) - (DSP_TX_FIRST_LINE + VRT_HEADER_WORDS + VRT_TRAILER_WORDS); //FIXME - //mem.ctrl_word = (VRT_HEADER_WORDS+p->items_per_frame+VRT_TRAILER_WORDS)*sizeof(uint32_t) | 1 << 16; + streaming_items_per_frame = (1500)/sizeof(uint32_t) - (DSP_TX_FIRST_LINE + VRT_HEADER_WORDS + VRT_TRAILER_WORDS); //FIXME + mem.ctrl_word = (VRT_HEADER_WORDS+streaming_items_per_frame+VRT_TRAILER_WORDS)*sizeof(uint32_t) | 1 << 16; memcpy_wa(buffer_ram(DSP_RX_BUF_0), &mem, sizeof(mem)); memcpy_wa(buffer_ram(DSP_RX_BUF_1), &mem, sizeof(mem)); //setup ethernet header machine - /*sr_udp_sm->eth_hdr.mac_dst_0_1 = (host_dst_mac_addr.addr[0] << 8) | host_dst_mac_addr.addr[1]; - sr_udp_sm->eth_hdr.mac_dst_2_3 = (host_dst_mac_addr.addr[2] << 8) | host_dst_mac_addr.addr[3]; - sr_udp_sm->eth_hdr.mac_dst_4_5 = (host_dst_mac_addr.addr[4] << 8) | host_dst_mac_addr.addr[5]; - sr_udp_sm->eth_hdr.mac_src_0_1 = (host_src_mac_addr.addr[0] << 8) | host_src_mac_addr.addr[1]; - sr_udp_sm->eth_hdr.mac_src_2_3 = (host_src_mac_addr.addr[2] << 8) | host_src_mac_addr.addr[3]; - sr_udp_sm->eth_hdr.mac_src_4_5 = (host_src_mac_addr.addr[4] << 8) | host_src_mac_addr.addr[5]; - sr_udp_sm->eth_hdr.ether_type = ETHERTYPE_IPV4;*/ + sr_udp_sm->eth_hdr.mac_dst_0_1 = (fp_mac_addr_dst.addr[0] << 8) | fp_mac_addr_dst.addr[1]; + sr_udp_sm->eth_hdr.mac_dst_2_3 = (fp_mac_addr_dst.addr[2] << 8) | fp_mac_addr_dst.addr[3]; + sr_udp_sm->eth_hdr.mac_dst_4_5 = (fp_mac_addr_dst.addr[4] << 8) | fp_mac_addr_dst.addr[5]; + sr_udp_sm->eth_hdr.mac_src_0_1 = (fp_mac_addr_src.addr[0] << 8) | fp_mac_addr_src.addr[1]; + sr_udp_sm->eth_hdr.mac_src_2_3 = (fp_mac_addr_src.addr[2] << 8) | fp_mac_addr_src.addr[3]; + sr_udp_sm->eth_hdr.mac_src_4_5 = (fp_mac_addr_src.addr[4] << 8) | fp_mac_addr_src.addr[5]; + sr_udp_sm->eth_hdr.ether_type = ETHERTYPE_IPV4; //setup ip header machine - /*unsigned int chksum = 0; + unsigned int chksum = 0; sr_udp_sm->ip_hdr.ver_ihl_tos = CHKSUM(0x4500, &chksum); // IPV4, 5 words of header (20 bytes), TOS=0 sr_udp_sm->ip_hdr.total_length = UDP_SM_INS_IP_LEN; // Don't checksum this line in SW sr_udp_sm->ip_hdr.identification = CHKSUM(0x0000, &chksum); // ID sr_udp_sm->ip_hdr.flags_frag_off = CHKSUM(0x4000, &chksum); // don't fragment sr_udp_sm->ip_hdr.ttl_proto = CHKSUM(0x2011, &chksum); // TTL=32, protocol = UDP (17 decimal) //sr_udp_sm->ip_hdr.checksum .... filled in below - uint32_t src_ip_addr = host_src_ip_addr.s_addr; - uint32_t dst_ip_addr = host_dst_ip_addr.s_addr; + uint32_t src_ip_addr = fp_socket_src.addr.addr; + uint32_t dst_ip_addr = fp_socket_dst.addr.addr; sr_udp_sm->ip_hdr.src_addr_high = CHKSUM(src_ip_addr >> 16, &chksum); // IP src high sr_udp_sm->ip_hdr.src_addr_low = CHKSUM(src_ip_addr & 0xffff, &chksum); // IP src low sr_udp_sm->ip_hdr.dst_addr_high = CHKSUM(dst_ip_addr >> 16, &chksum); // IP dst high @@ -591,17 +624,14 @@ start_rx_streaming_cmd(void *p) sr_udp_sm->ip_hdr.checksum = UDP_SM_INS_IP_HDR_CHKSUM | (chksum & 0xffff); //setup the udp header machine - sr_udp_sm->udp_hdr.src_port = host_src_udp_port; - sr_udp_sm->udp_hdr.dst_port = host_dst_udp_port; + sr_udp_sm->udp_hdr.src_port = fp_socket_src.port; + sr_udp_sm->udp_hdr.dst_port = fp_socket_dst.port; sr_udp_sm->udp_hdr.length = UDP_SM_INS_UDP_LEN; - sr_udp_sm->udp_hdr.checksum = UDP_SM_LAST_WORD; // zero UDP checksum*/ + sr_udp_sm->udp_hdr.checksum = UDP_SM_LAST_WORD; // zero UDP checksum if (FW_SETS_SEQNO) fw_seqno = 0; - //streaming_items_per_frame = p->items_per_frame; - //time_secs = p->time_secs; - //time_ticks = p->time_ticks; restart_streaming(); } diff --git a/host/lib/usrp/usrp2/io_impl.cpp b/host/lib/usrp/usrp2/io_impl.cpp index fbea71f85..4781036ea 100644 --- a/host/lib/usrp/usrp2/io_impl.cpp +++ b/host/lib/usrp/usrp2/io_impl.cpp @@ -37,25 +37,6 @@ static const size_t max_vrt_header_words = 5; /*********************************************************************** * Helper Functions **********************************************************************/ - -/*static void pack_vrt_header( - size_t num_data_words, //input - const uhd::metadata_t &metadata, //input - uint32_t *vrt_header, //output - size_t &vrt_header_words //output -){ - // -} - -static void unpack_vrt_header( - size_t &num_data_words, //output - uhd::metadata_t &metadata, //output - const uint32_t *vrt_header, //input - size_t &vrt_header_words //output -){ - // -}*/ - static inline void host_floats_to_usrp2_shorts( int16_t *usrp2_shorts, const float *host_floats, -- cgit v1.2.3 From c12d6d0f5f2bbfd749b5a18b167ed7a485cd03a1 Mon Sep 17 00:00:00 2001 From: Josh Blum Date: Tue, 23 Feb 2010 18:16:34 -0800 Subject: vrt packet count fix --- host/lib/usrp/usrp2/io_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (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 4781036ea..43334ddc6 100644 --- a/host/lib/usrp/usrp2/io_impl.cpp +++ b/host/lib/usrp/usrp2/io_impl.cpp @@ -105,7 +105,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]++ << 16) & 0xf) | + ((_stream_id_to_packet_seq[metadata.stream_id]++ & 0xf) << 16) | ((boost::asio::buffer_size(buff)/sizeof(uint32_t)) & 0xffff) ); -- cgit v1.2.3