[Commit-gnuradio] r5884 - gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband

[gnychis]

Author: gnychis
Date: 2007-07-02 11:42:40 -0600 (Mon, 02 Jul 2007)
New Revision: 5884

Modified:
   
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.cc
   
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.h
   
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_inband_usb_packet.h
   
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.cc
   
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.h
   
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_tx_stub.cc
Log:
Checking in code for 4 new CS packets:
  - OP_WRITE_REG
  - OP_WRITE_REG_MASKED
  - OP_READ_REG
  - OP_READ_REG_REPLY

Wrote some QA code for these packets and check that the data is set correctly
using output and a hex editor until I can better automate it.


Modified: 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.cc
===================================================================
--- 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.cc
   2007-07-02 15:14:42 UTC (rev 5883)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.cc
   2007-07-02 17:42:40 UTC (rev 5884)
@@ -1105,7 +1105,7 @@
   : mb_mblock(runtime, instance_name, user_arg)
 { 
   d_nrecvd=0;
-  d_nmsgs_to_recv = 4;
+  d_nmsgs_to_recv = 10;
   d_nstatus=0;
   d_nstatus_to_recv = 50;
   
@@ -1165,6 +1165,29 @@
                                       pmt_list2(PMT_NIL, 
                                       pmt_from_long(0))))));
   
+  d_tx->send(s_cmd_to_control_channel,
+             pmt_list2(pmt_list2(s_response_from_control_channel, PMT_T),
+                       pmt_list1(
+                            pmt_list2(s_op_write_reg, 
+                                      pmt_list2(
+                                      pmt_from_long(0x3), 
+                                      pmt_from_long(0x4))))));
+  
+  d_tx->send(s_cmd_to_control_channel,
+             pmt_list2(pmt_list2(s_response_from_control_channel, PMT_T),
+                       pmt_list1(
+                            pmt_list2(s_op_write_reg_masked, 
+                                      pmt_list3(
+                                      pmt_from_long(0x3), 
+                                      pmt_from_long(0x4),
+                                      pmt_from_long(0x5))))));
+  
+  d_tx->send(s_cmd_to_control_channel,
+             pmt_list2(pmt_list2(s_response_from_control_channel, PMT_T),
+                       pmt_list1(
+                            pmt_list2(s_op_read_reg, 
+                                      pmt_list2(PMT_NIL, 
+                                      pmt_from_long(0x6))))));
 }
 
 void

Modified: 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.h
===================================================================
--- 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.h
    2007-07-02 15:14:42 UTC (rev 5883)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/qa_inband_usrp_server.h
    2007-07-02 17:42:40 UTC (rev 5884)
@@ -28,11 +28,11 @@
 class qa_inband_usrp_server : public CppUnit::TestCase {
 
   CPPUNIT_TEST_SUITE(qa_inband_usrp_server);
-  CPPUNIT_TEST(test_open_close);
-  CPPUNIT_TEST(test_chan_allocation);
-  CPPUNIT_TEST(test_chan_deallocation);
-  CPPUNIT_TEST(test_tx);
-  CPPUNIT_TEST(test_rx);
+//  CPPUNIT_TEST(test_open_close);
+//  CPPUNIT_TEST(test_chan_allocation);
+//  CPPUNIT_TEST(test_chan_deallocation);
+//  CPPUNIT_TEST(test_tx);
+//  CPPUNIT_TEST(test_rx);
   CPPUNIT_TEST(test_cs);
   CPPUNIT_TEST_SUITE_END();
 

Modified: 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_inband_usb_packet.h
===================================================================
--- 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_inband_usb_packet.h
   2007-07-02 15:14:42 UTC (rev 5883)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_inband_usb_packet.h
   2007-07-02 17:42:40 UTC (rev 5884)
@@ -25,6 +25,7 @@
 #include <usrp_bytesex.h>
 #include <mb_mblock.h>
 #include <pmt.h>
+#include <iostream>
 
 #include <symbols_usrp_low_level_cs.h>
 
@@ -97,6 +98,9 @@
   static const int CS_PINGVAL_MASK = 0x3ff;
   static const int CS_PINGVAL_SHIFT = 0x0;
 
+  static const int CS_REGNUM_MASK = 0x3ff;
+  static const int CS_REGNUM_SHIFT = 0x0;
+
 public:
   
   void set_timestamp(uint32_t timestamp){
@@ -179,8 +183,8 @@
     return USB_PKT_SIZE;
   }
 
-  uint32_t cs_ping(long rid, long ping_val) {
-
+  void cs_ping(long rid, long ping_val)
+  {
     uint32_t ping = ( 
         ((OP_PING_FIXED & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
       | ((2 & CS_LENGTH_MASK) << CS_LENGTH_SHIFT)
@@ -200,17 +204,15 @@
     int h_payload_len = payload_len() + 4;  // increased by fixed ping size
 
     set_header(h_flags, h_chan, h_tag, h_payload_len);
-
-    return ping;
   }
   
-  uint32_t cs_ping_reply(long rid, long ping_val) {
-
+  uint32_t cs_ping_reply(long rid, long ping_val) 
+  {
     uint32_t ping = ( 
         ((OP_PING_FIXED_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
       | ((0x2 & CS_LENGTH_MASK) << CS_LENGTH_SHIFT)
       | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
-      | (ping_val & CS_PINGVAL_MASK)
+      | ((ping_val & CS_PINGVAL_MASK) << CS_PINGVAL_SHIFT)
 
       );
 
@@ -229,6 +231,119 @@
     return ping;
   }
 
+  void cs_write_reg(long reg_num, long val)
+  {
+    uint32_t word0 = 0;
+
+    // Build the first word which includes the register number
+    word0 = (
+        ((OP_WRITE_REG & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
+      | ((6 & CS_LENGTH_MASK) << CS_LENGTH_SHIFT)
+      | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
+    );
+
+    int p_len = payload_len();
+    uint32_t *payload = (uint32_t *) d_payload + p_len;
+    *payload = host_to_usrp_u32(word0);
+
+    // The second word is solely the register value to be written
+    // FIXME: should this be unsigned?
+    payload += 4;  // skip past the first word
+    *payload = host_to_usrp_u32((uint32_t) val);
+    
+    // Rebuild the header to update the payload length
+    int h_flags = flags();
+    int h_chan = chan();
+    int h_tag = tag();
+    int h_payload_len = payload_len() + 8;  // added 2 words
+
+    set_header(h_flags, h_chan, h_tag, h_payload_len);
+  }
+  
+  void cs_write_reg_masked(long reg_num, long val, long mask)
+  {
+    uint32_t word0 = 0;
+
+    // Build the first word which includes the register number
+    word0 = (
+        ((OP_WRITE_REG_MASKED & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
+      | ((10 & CS_LENGTH_MASK) << CS_LENGTH_SHIFT)
+      | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
+    );
+
+    int p_len = payload_len();
+    uint32_t *payload = (uint32_t *) d_payload + p_len;
+    *payload = host_to_usrp_u32(word0);
+
+    // Skip over the first word and write the register value
+    payload += 4;
+    *payload = host_to_usrp_u32((uint32_t) val);
+
+    // Skip over the register value and write the mask
+    payload += 4;
+    *payload = host_to_usrp_u32((uint32_t) mask);
+    
+    // Rebuild the header to update the payload length
+    int h_flags = flags();
+    int h_chan = chan();
+    int h_tag = tag();
+    int h_payload_len = payload_len() + 12;  // added 2 words
+
+    set_header(h_flags, h_chan, h_tag, h_payload_len);
+  }
+  
+  void cs_read_reg(long rid, long reg_num)
+  {
+
+    uint32_t read_reg = ( 
+        ((OP_READ_REG & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
+      | ((2 & CS_LENGTH_MASK) << CS_LENGTH_SHIFT)
+      | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
+      | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
+
+      );
+
+    int p_len = payload_len();
+    uint32_t *payload = (uint32_t *) d_payload + p_len;
+    *payload = host_to_usrp_u32(read_reg);
+
+    // Update payload length
+    int h_flags = flags();
+    int h_chan = chan();
+    int h_tag = tag();
+    int h_payload_len = payload_len() + 4;
+
+    set_header(h_flags, h_chan, h_tag, h_payload_len);
+  }
+  
+  void cs_read_reg_reply(long rid, long reg_num, long reg_val)
+  {
+
+    uint32_t word0 = ( 
+        ((OP_READ_REG_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
+      | ((6 & CS_LENGTH_MASK) << CS_LENGTH_SHIFT)
+      | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
+      | ((reg_num & CS_REGNUM_MASK) << CS_REGNUM_SHIFT)
+
+      );
+
+    int p_len = payload_len();
+    uint32_t *payload = (uint32_t *) d_payload + p_len;
+    *payload = host_to_usrp_u32(word0);
+
+    // Hop to the next word and write the reg value
+    payload += 4;
+    *payload = host_to_usrp_u32((uint32_t) reg_val); 
+
+    // Update payload length
+    int h_flags = flags();
+    int h_chan = chan();
+    int h_tag = tag();
+    int h_payload_len = payload_len() + 8;
+
+    set_header(h_flags, h_chan, h_tag, h_payload_len);
+  }
+
   // The following method takes an offset within the packet payload to extract
   // a control/status subpacket and construct a pmt response which includes the
   // proper signal and arguments specified by usrp-low-level-cs.  The USRP
@@ -246,14 +361,23 @@
       
       case OP_PING_FIXED_REPLY:
       {
-        pmt_t rid     = pmt_from_long((subpkt & CS_RID_MASK) << CS_RID_SHIFT);
-        pmt_t pingval = pmt_from_long((subpkt & CS_PINGVAL_MASK) << 
CS_PINGVAL_SHIFT);
+        pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
+        pmt_t pingval = pmt_from_long((subpkt >> CS_PINGVAL_SHIFT) & 
CS_PINGVAL_MASK);
         return pmt_list3(s_op_ping_fixed_reply, rid, pingval);
       }
 
       case OP_READ_REG_REPLY:
-        return PMT_NIL;
+      {
+        pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
+        pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & 
CS_REGNUM_MASK);
 
+        // To get the register value we just read the next 32 bits
+        uint32_t val  = usrp_to_host_u32(*((uint32_t *)d_payload + 
payload_offset + 4));
+        pmt_t reg_val = pmt_from_long(val);
+
+        return pmt_list4(s_op_read_reg_reply, rid, reg_num, reg_val);
+      }
+
       case OP_I2C_READ_REPLY:
         return PMT_NIL;
 
@@ -262,8 +386,8 @@
 
       case OP_PING_FIXED:
       {
-        pmt_t rid     = pmt_from_long((subpkt & CS_RID_MASK) << CS_RID_SHIFT);
-        pmt_t pingval = pmt_from_long((subpkt & CS_PINGVAL_MASK) << 
CS_PINGVAL_SHIFT);
+        pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
+        pmt_t pingval = pmt_from_long((subpkt >> CS_PINGVAL_SHIFT) & 
CS_PINGVAL_MASK);
         return pmt_list3(s_op_ping_fixed, rid, pingval);
       }
 
@@ -274,8 +398,13 @@
         return PMT_NIL;
 
       case OP_READ_REG:
-        return PMT_NIL;
+      {
+        pmt_t rid     = pmt_from_long((subpkt >> CS_RID_SHIFT) & CS_RID_MASK);
+        pmt_t reg_num = pmt_from_long((subpkt >> CS_REGNUM_SHIFT) & 
CS_REGNUM_MASK);
 
+        return pmt_list3(s_op_read_reg, rid, reg_num);
+      }
+
       case OP_I2C_WRITE:
         return PMT_NIL;
 

Modified: 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.cc
===================================================================
--- 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.cc 
    2007-07-02 15:14:42 UTC (rev 5883)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.cc 
    2007-07-02 17:42:40 UTC (rev 5884)
@@ -98,11 +98,15 @@
   d_rx_chan_mask = 0;
 
   // Initialize request ID's to 0
-  d_op_ping_fixed_rid = 0;
+  d_op_ping_fixed_rid = 2;
+  d_op_read_reg_rid = 1;
 
   for(int i=0; i < D_OP_PING_FIXED_MAX_RID; i++)
     d_op_ping_fixed_owners.push_back(PMT_NIL);
 
+  for(int i=0; i < D_OP_READ_REG_MAX_RID; i++)
+    d_op_read_reg_owners.push_back(PMT_NIL);
+
   //d_fake_rx=true;
 }
 
@@ -706,13 +710,54 @@
       // Adds a ping after the previous command in the pkt
       pkt->cs_ping(rid, pingval);
 
+      if(verbose)
+        std::cout << "[USRP_SERVER] Received ping command request"
+                  << " assigning RID " << rid << std::endl;
+
     }
+  
+    //----------- WRITE REG ---------------//
+    if(pmt_eq(subp_cmd, s_op_write_reg)) {
+      
+      long reg_num = pmt_to_long(pmt_nth(0, subp_data));
+      long val = pmt_to_long(pmt_nth(1, subp_data));
 
+      pkt->cs_write_reg(reg_num, val);
+      
+      if(verbose)
+        std::cout << "[USRP_SERVER] Received write register request\n";
+    }
+    
+    //------- WRITE REG MASKED ----------//
+    if(pmt_eq(subp_cmd, s_op_write_reg_masked)) {
+      
+      long reg_num = pmt_to_long(pmt_nth(0, subp_data));
+      long val = pmt_to_long(pmt_nth(1, subp_data));
+      long mask = pmt_to_long(pmt_nth(2, subp_data));
+
+      pkt->cs_write_reg_masked(reg_num, val, mask);
+      
+      if(verbose)
+        std::cout << "[USRP_SERVER] Received write register masked request\n";
+    }
+    
+    //------------ READ REG --------------//
+    if(pmt_eq(subp_cmd, s_op_read_reg)) {
+      
+      long reg_num = pmt_to_long(pmt_nth(1, subp_data));
+      long rid = d_op_read_reg_rid++ % D_OP_PING_FIXED_MAX_RID;
+
+      pkt->cs_read_reg(rid, reg_num);
+      
+      if(verbose)
+        std::cout << "[USRP_SERVER] Received read register request"
+                  << " assigning RID " << rid << std::endl;
+    }
+
     d_cs_usrp->send(s_cmd_usrp_write, 
                     pmt_list3(invocation_handle, 
                               pmt_from_long(channel), 
                               v_packet));
-
   }
 
   return;
@@ -865,9 +910,6 @@
   long curr_payload = 0;
   long port;
   
-  if(verbose)
-    std::cout << "[USRP_SERVER] Received control/status packet, parsing...\n";
-
   // We dispatch based on the control packet type, however we can extract the
   // opcode and the length immediately which is consistent in all responses.
   //
@@ -889,6 +931,16 @@
 
       pmt_t rid = pmt_nth(1, sub_packet);
       pmt_t pingval = pmt_nth(2, sub_packet);
+      
+      if(verbose)
+        std::cout << "[USRP_SERVER] Found ping response: \n"
+                  << "\tRID: " << rid << std::endl
+                  << "\tVAL: " << pingval << std::endl;
+      
+      // Do some bounds checking incase of bogus/corrupt responses
+      if(pmt_to_long(rid) > D_OP_PING_FIXED_MAX_RID)
+        return;
+
       pmt_t owner = d_op_ping_fixed_owners[pmt_to_long(rid)];
 
       // FIXME: should be 1 response for all subpackets here ?
@@ -899,9 +951,35 @@
                                    pmt_list2(s_op_ping_fixed_reply, // subp
                                              pmt_list2(rid, pingval))));
 
+    }
+    
+    //----------------- READ REG RESPONSE ------------------//
+    else if(pmt_eq(op_symbol, s_op_read_reg_reply)) {
+
+      pmt_t rid = pmt_nth(1, sub_packet);
+      pmt_t reg_num = pmt_nth(2, sub_packet);
+      pmt_t reg_val = pmt_nth(3, sub_packet);
+      
       if(verbose)
-        std::cout << "[USRP_SERVER] Sending control channel response...\n";
+        std::cout << "[USRP_SERVER] Found read register response: \n"
+                  << "\tRID: " << rid << std::endl
+                  << "\tREG: " << reg_num << std::endl
+                  << "\tVAL: " << reg_val << std::endl;
 
+      // Do some bounds checking to avoid seg faults
+      if(pmt_to_long(rid) > D_OP_READ_REG_MAX_RID)
+        return;
+      
+      pmt_t owner = d_op_read_reg_owners[pmt_to_long(rid)];
+
+      // FIXME: should be 1 response for all subpackets here ?
+      if((port = tx_port_index(owner)) != -1)
+        d_tx[port]->send(s_response_from_control_channel,
+                         pmt_list3(invocation_handle,
+                                   PMT_T,
+                                   pmt_list2(s_op_read_reg_reply, // subp
+                                             pmt_list3(rid, reg_num, 
reg_val))));
+
     }
 
     // Each subpacket has an unaccounted for 2 bytes which is the opcode

Modified: 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.h
===================================================================
--- 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.h  
    2007-07-02 15:14:42 UTC (rev 5883)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.h  
    2007-07-02 17:42:40 UTC (rev 5884)
@@ -60,8 +60,12 @@
 
   // USRP server assigns and keeps track of request IDs.  
   long d_op_ping_fixed_rid;
-  static const long D_OP_PING_FIXED_MAX_RID = 6;
+  static const long D_OP_PING_FIXED_MAX_RID = 64;
   std::vector<pmt_t> d_op_ping_fixed_owners;
+  
+  long d_op_read_reg_rid;
+  static const long D_OP_READ_REG_MAX_RID = 64;
+  std::vector<pmt_t> d_op_read_reg_owners;
 
 
   struct channel_info {

Modified: 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_tx_stub.cc
===================================================================
--- 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_tx_stub.cc
    2007-07-02 15:14:42 UTC (rev 5883)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_tx_stub.cc
    2007-07-02 17:42:40 UTC (rev 5884)
@@ -166,9 +166,39 @@
       d_cs_queue.push(pmt_list2(invocation_handle, v_pkt));
 
       if(verbose)
-        std::cout << "[USRP_TX_STUB] Generated ping response\n";
+        std::cout << "[USRP_TX_STUB] Generated ping response:\n"
+                  << "\tRID: " << rid << std::endl
+                  << "\tVAL: " << pingval << std::endl;
     }
 
+    //----------------- READ REG ------------------//
+    if(pmt_eq(op_symbol, s_op_read_reg)) {
+
+      size_t ignore;
+
+      long rid = pmt_to_long(pmt_nth(1, sub_packet));
+      long reg_num = pmt_to_long(pmt_nth(2, sub_packet));
+      long reg_val = 0xdeef;
+
+      pmt_t v_pkt = pmt_make_u8vector(sizeof(transport_pkt), 0);
+      
+      transport_pkt *q_pkt =
+        (transport_pkt *) pmt_u8vector_writeable_elements(v_pkt, ignore);
+
+      q_pkt->set_header(0, 0x1f, 0, 0);
+      q_pkt->set_timestamp(0xffffffff);
+
+      // Generate a reply and put it in the queue for the RX stub to read
+      q_pkt->cs_read_reg_reply(rid, reg_num, reg_val);
+      d_cs_queue.push(pmt_list2(invocation_handle, v_pkt));
+
+      if(verbose)
+        std::cout << "[USRP_TX_STUB] Generated read register response:\n"
+                  << "\tRID: " << rid << std::endl
+                  << "\tREG: " << reg_num << std::endl
+                  << "\tVAL: " << reg_val << std::endl;
+    }
+
     // Each subpacket has an unaccounted for 2 bytes which is the opcode
     // and the length field
     curr_payload += len + 2;