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;