diff options
| -rw-r--r-- | host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp | 155 |
1 files changed, 90 insertions, 65 deletions
diff --git a/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp b/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp index d6e68c636..0f652b294 100644 --- a/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp +++ b/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp @@ -47,7 +47,8 @@ public: usrp2_fifo_ctrl_impl(zero_copy_if::sptr xport): _xport(xport), - _seq(0), _last_seq_ack(0), + _seq_out(0), + _seq_ack(0), _timeout(ACK_TIMEOUT) { while (_xport->get_recv_buff(0.0)){} //flush @@ -63,48 +64,15 @@ public: ) } - UHD_INLINE void send_pkt(wb_addr_type addr, boost::uint32_t data, int cmd){ - managed_send_buffer::sptr buff = _xport->get_send_buff(0.0); - if (not buff){ - throw uhd::runtime_error("fifo ctrl timed out getting a send buffer"); - } - boost::uint32_t *trans = buff->cast<boost::uint32_t *>(); - trans[0] = htonl(++_seq); - boost::uint32_t *pkt = trans + 1; - - //load packet info - vrt::if_packet_info_t packet_info; - packet_info.packet_type = vrt::if_packet_info_t::PACKET_TYPE_CONTEXT; - packet_info.num_payload_words32 = 2; - packet_info.num_payload_bytes = packet_info.num_payload_words32*sizeof(boost::uint32_t); - packet_info.packet_count = _seq; - packet_info.tsf = _time.to_ticks(_tick_rate); - packet_info.sob = false; - packet_info.eob = false; - packet_info.has_sid = false; - packet_info.has_cid = false; - packet_info.has_tsi = false; - packet_info.has_tsf = _use_time; - packet_info.has_tlr = false; - - //load header - vrt::if_hdr_pack_be(pkt, packet_info); - - //load payload - const boost::uint32_t ctrl_word = (addr & 0xff) | cmd | (_seq << 16); - pkt[packet_info.num_header_words32+0] = htonl(ctrl_word); - pkt[packet_info.num_header_words32+1] = htonl(data); - - //send the buffer over the interface - buff->commit(sizeof(boost::uint32_t)*(packet_info.num_packet_words32+1)); - } - + /******************************************************************* + * Peek and poke 32 bit implementation + ******************************************************************/ void poke32(wb_addr_type addr, boost::uint32_t data){ boost::mutex::scoped_lock lock(_mutex); this->send_pkt((addr - SETTING_REGS_BASE)/4, data, POKE32_CMD); - this->wait_for_ack(boost::int16_t(_seq-MAX_SEQS_OUT)); + this->wait_for_ack(_seq_out-MAX_SEQS_OUT); } boost::uint32_t peek32(wb_addr_type addr){ @@ -112,29 +80,12 @@ public: this->send_pkt((addr - READBACK_BASE)/4, 0, PEEK32_CMD); - return this->wait_for_ack(boost::int16_t(_seq)); - } - - UHD_INLINE boost::uint32_t wait_for_ack(const boost::int16_t seq_to_ack){ - - while (_last_seq_ack < seq_to_ack){ - managed_recv_buffer::sptr buff = _xport->get_recv_buff(_timeout); - if (not buff){ - throw uhd::runtime_error("fifo ctrl timed out looking for acks"); - } - const boost::uint32_t *pkt = buff->cast<const boost::uint32_t *>(); - vrt::if_packet_info_t packet_info; - packet_info.num_packet_words32 = buff->size()/sizeof(boost::uint32_t); - vrt::if_hdr_unpack_be(pkt, packet_info); - _last_seq_ack = ntohl(pkt[packet_info.num_header_words32+0]) >> 16; - if (_last_seq_ack == seq_to_ack){ - return ntohl(pkt[packet_info.num_header_words32+1]); - } - } - - return 0; + return this->wait_for_ack(_seq_out); } + /******************************************************************* + * Peek and poke 16 bit not implemented + ******************************************************************/ void poke16(wb_addr_type, boost::uint16_t){ throw uhd::not_implemented_error("poke16 not implemented in fifo ctrl module"); } @@ -143,11 +94,14 @@ public: throw uhd::not_implemented_error("peek16 not implemented in fifo ctrl module"); } + /******************************************************************* + * FIFO controlled SPI implementation + ******************************************************************/ void init_spi(void){ boost::mutex::scoped_lock lock(_mutex); this->send_pkt(SPI_DIV, SPI_DIVIDER, POKE32_CMD); - this->wait_for_ack(boost::int16_t(_seq-MAX_SEQS_OUT)); + this->wait_for_ack(_seq_out-MAX_SEQS_OUT); _ctrl_word_cache = 0; // force update first time around } @@ -174,23 +128,26 @@ public: //conditionally send control word if (_ctrl_word_cache != ctrl_word){ this->send_pkt(SPI_CTRL, ctrl_word, POKE32_CMD); - this->wait_for_ack(boost::int16_t(_seq-MAX_SEQS_OUT)); + this->wait_for_ack(_seq_out-MAX_SEQS_OUT); _ctrl_word_cache = ctrl_word; } //send data word this->send_pkt(SPI_DATA, data_out, POKE32_CMD); - this->wait_for_ack(boost::int16_t(_seq-MAX_SEQS_OUT)); + this->wait_for_ack(_seq_out-MAX_SEQS_OUT); //conditional readback if (readback){ this->send_pkt(SPI_READBACK, 0, PEEK32_CMD); - return this->wait_for_ack(boost::int16_t(_seq)); + return this->wait_for_ack(_seq_out); } return 0; } + /******************************************************************* + * Update methods for time + ******************************************************************/ void set_time(const uhd::time_spec_t &time){ boost::mutex::scoped_lock lock(_mutex); _time = time; @@ -204,10 +161,78 @@ public: } private: + + /******************************************************************* + * Primary control and interaction private methods + ******************************************************************/ + UHD_INLINE void send_pkt(wb_addr_type addr, boost::uint32_t data, int cmd){ + managed_send_buffer::sptr buff = _xport->get_send_buff(0.0); + if (not buff){ + throw uhd::runtime_error("fifo ctrl timed out getting a send buffer"); + } + boost::uint32_t *trans = buff->cast<boost::uint32_t *>(); + trans[0] = htonl(++_seq_out); + boost::uint32_t *pkt = trans + 1; + + //load packet info + vrt::if_packet_info_t packet_info; + packet_info.packet_type = vrt::if_packet_info_t::PACKET_TYPE_CONTEXT; + packet_info.num_payload_words32 = 2; + packet_info.num_payload_bytes = packet_info.num_payload_words32*sizeof(boost::uint32_t); + packet_info.packet_count = _seq_out; + packet_info.tsf = _time.to_ticks(_tick_rate); + packet_info.sob = false; + packet_info.eob = false; + packet_info.has_sid = false; + packet_info.has_cid = false; + packet_info.has_tsi = false; + packet_info.has_tsf = _use_time; + packet_info.has_tlr = false; + + //load header + vrt::if_hdr_pack_be(pkt, packet_info); + + //load payload + const boost::uint32_t ctrl_word = (addr & 0xff) | cmd | (_seq_out << 16); + pkt[packet_info.num_header_words32+0] = htonl(ctrl_word); + pkt[packet_info.num_header_words32+1] = htonl(data); + + //send the buffer over the interface + buff->commit(sizeof(boost::uint32_t)*(packet_info.num_packet_words32+1)); + } + + UHD_INLINE bool wraparound_lt16(const boost::int16_t i0, const boost::int16_t i1){ + if (i0 == i1) return false; + const int s0 = boost::uint16_t(i0) >> 15; + const int s1 = boost::uint16_t(i1) >> 15; + if (s0 != s1) return boost::int16_t(i1 - i0) > 0; + return boost::uint16_t(i0) < boost::uint16_t(i1); + } + + UHD_INLINE boost::uint32_t wait_for_ack(const boost::uint16_t seq_to_ack){ + + while (wraparound_lt16(_seq_ack, seq_to_ack)){ + managed_recv_buffer::sptr buff = _xport->get_recv_buff(_timeout); + if (not buff){ + throw uhd::runtime_error("fifo ctrl timed out looking for acks"); + } + const boost::uint32_t *pkt = buff->cast<const boost::uint32_t *>(); + vrt::if_packet_info_t packet_info; + packet_info.num_packet_words32 = buff->size()/sizeof(boost::uint32_t); + vrt::if_hdr_unpack_be(pkt, packet_info); + _seq_ack = ntohl(pkt[packet_info.num_header_words32+0]) >> 16; + if (_seq_ack == seq_to_ack){ + return ntohl(pkt[packet_info.num_header_words32+1]); + } + } + + return 0; + } + zero_copy_if::sptr _xport; boost::mutex _mutex; - boost::uint32_t _seq; - boost::uint16_t _last_seq_ack; + boost::uint16_t _seq_out; + boost::uint16_t _seq_ack; uhd::time_spec_t _time; bool _use_time; double _tick_rate; |