1 files changed, 24 insertions, 24 deletions

diff --git a/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp b/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp
index 4691fa887..e19e7d5e7 100644
--- a/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp
+++ b/host/lib/usrp/usrp2/usrp2_fifo_ctrl.cpp
@@ -15,6 +15,7 @@
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
 //
 
+#include "usrp2_regs.hpp"
 #include <uhd/exception.hpp>
 #include <uhd/utils/msg.hpp>
 #include <uhd/transport/vrt_if_packet.hpp>
@@ -27,24 +28,25 @@ using namespace uhd::transport;
 static const size_t POKE32_CMD = (1 << 8);
 static const size_t PEEK32_CMD = 0;
 static const double ACK_TIMEOUT = 0.5;
+static const boost::uint32_t MAX_SEQS_OUT = 16;
 
 class usrp2_fifo_ctrl_impl : public usrp2_fifo_ctrl{
 public:
 
     usrp2_fifo_ctrl_impl(zero_copy_if::sptr xport):
         _xport(xport),
-        _seq(0)
+        _seq(0), _last_seq_ack(0)
     {
-        //NOP
+        while (_xport->get_recv_buff(0.0)){} //flush
     }
 
     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("peek32/poke32 in fifo ctrl timed out getting a send buffer");
+            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++);
+        trans[0] = htonl(++_seq);
         boost::uint32_t *pkt = trans + 1;
 
         //load packet info
@@ -61,10 +63,10 @@ public:
         packet_info.has_tsf = false;
         packet_info.has_tlr = false;
 
-        //load header with offset 1
+        //load header
         vrt::if_hdr_pack_be(pkt, packet_info);
 
-        //load payload with offset 1
+        //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);
@@ -76,40 +78,37 @@ public:
     void poke32(wb_addr_type addr, boost::uint32_t data){
         boost::mutex::scoped_lock lock(_mutex);
 
-        while (_xport->get_recv_buff(0.0)){} //flush
-
-        this->send_pkt(addr, data, POKE32_CMD);
+        this->send_pkt((addr - SETTING_REGS_BASE)/4, data, POKE32_CMD);
 
-        {
-            managed_recv_buffer::sptr buff = _xport->get_recv_buff(ACK_TIMEOUT);
-            if (not buff){
-                throw uhd::runtime_error("poke32 in fifo ctrl timed out getting a recv buffer");
-            }
-            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);
-        }
+        this->wait_for_ack(boost::int16_t(_seq-MAX_SEQS_OUT));
     }
 
     boost::uint32_t peek32(wb_addr_type addr){
         boost::mutex::scoped_lock lock(_mutex);
 
-        while (_xport->get_recv_buff(0.0)){} //flush
+        this->send_pkt((addr - READBACK_BASE)/4, 0, PEEK32_CMD);
 
-        this->send_pkt(addr >> 2, 0, PEEK32_CMD);
+        return this->wait_for_ack(boost::int16_t(_seq));
+    }
+
+    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(ACK_TIMEOUT);
             if (not buff){
-                throw uhd::runtime_error("peek32 in fifo ctrl timed out getting a recv buffer");
+                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);
-            return ntohl(pkt[packet_info.num_header_words32+1]);
+            _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;
     }
 
     void poke16(wb_addr_type, boost::uint16_t){
@@ -124,6 +123,7 @@ private:
     zero_copy_if::sptr _xport;
     boost::mutex _mutex;
     boost::uint32_t _seq;
+    boost::uint16_t _last_seq_ack;
 };