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

[gnychis]

Author: gnychis
Date: 2007-07-05 12:40:58 -0600 (Thu, 05 Jul 2007)
New Revision: 5906

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_server.cc
   
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_tx_stub.cc
Log:
Added in fake replys to I2C reads in the TX and RX stub.

USRP server should now correctly parse I2C read replies, will test further
before going on the SPI C/S code.


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-04 22:58:57 UTC (rev 5905)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_inband_usb_packet.h
   2007-07-05 18:40:58 UTC (rev 5906)
@@ -116,9 +116,9 @@
   static const int CS_I2CADDR_MASK = 0x7f;
   static const int CS_I2CADDR_SHIFT = 0;
 
-  static const int CS_I2CWRITE_LEN = 6;
-  static const int CS_I2CWRITEBYTES_MASK = 0x7f;
-  static const int CS_I2CWRITEBYTES_SHIFT = 24;
+  static const int CS_I2CREAD_LEN = 6;
+  static const int CS_I2CREADBYTES_MASK = 0x7f;
+  static const int CS_I2CREADBYTES_SHIFT = 24;
 
 public:
   
@@ -464,14 +464,14 @@
   {
     int p_len = payload_len();
 
-    if((MAX_PAYLOAD - p_len) < (CS_I2CWRITE_LEN + CS_FIXED_LEN))
+    if((MAX_PAYLOAD - p_len) < (CS_I2CREAD_LEN + CS_FIXED_LEN))
       return false;
 
     uint32_t word0 = 0;
     
     word0 = ( 
-        ((OP_READ_REG & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
-      | ((CS_I2CWRITE_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
+        ((OP_I2C_READ & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
+      | ((CS_I2CREAD_LEN & CS_LEN_MASK) << CS_LEN_SHIFT)
       | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
       | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT)
       );
@@ -482,20 +482,56 @@
     // Jump a word and write the number of bytes to read
     payload += 4;
     uint32_t word1 = 
-      (n_bytes & CS_I2CWRITEBYTES_MASK) << CS_I2CWRITEBYTES_SHIFT;
+      (n_bytes & CS_I2CREADBYTES_MASK) << CS_I2CREADBYTES_SHIFT;
     *payload = host_to_usrp_u32(word1);
 
     // Update payload length
     int h_flags = flags();
     int h_chan = chan();
     int h_tag = tag();
-    int h_payload_len = payload_len() + CS_FIXED_LEN + CS_I2CWRITE_LEN;
+    int h_payload_len = payload_len() + CS_FIXED_LEN + CS_I2CREAD_LEN;
 
     set_header(h_flags, h_chan, h_tag, h_payload_len);
 
     return true;
   }
+  
+  bool cs_i2c_read_reply(long rid, long i2c_addr, uint8_t *i2c_data, long 
i2c_data_len)
+  {
+    int p_len = payload_len();
 
+    int i2c_len = i2c_data_len + 2;
+
+    if((MAX_PAYLOAD - p_len) < (i2c_len + CS_FIXED_LEN))
+      return false;
+    
+    uint32_t word0 = 0;
+    
+    word0 = ( 
+        ((OP_I2C_READ_REPLY & CS_OPCODE_MASK) << CS_OPCODE_SHIFT)
+      | ((i2c_len & CS_LEN_MASK) << CS_LEN_SHIFT)
+      | ((rid & CS_RID_MASK) << CS_RID_SHIFT)
+      | ((i2c_addr & CS_I2CADDR_MASK) << CS_I2CADDR_SHIFT)
+      );
+
+    uint32_t *payload = (uint32_t *) d_payload + p_len;
+    *payload = host_to_usrp_u32(word0);
+
+    // Jump a word and write the actual data
+    payload += 4;
+    memcpy(payload, i2c_data, i2c_data_len);
+
+    // Update payload length
+    int h_flags = flags();
+    int h_chan = chan();
+    int h_tag = tag();
+    int h_payload_len = payload_len() + CS_FIXED_LEN + i2c_len;
+
+    set_header(h_flags, h_chan, h_tag, h_payload_len);
+
+    return true;
+  }
+
   // 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
@@ -531,8 +567,21 @@
       }
 
       case OP_I2C_READ_REPLY:
-        return PMT_NIL;
+      {
+        pmt_t rid       = pmt_from_long((subpkt >> CS_RID_SHIFT) & 
CS_RID_MASK);
+        pmt_t i2c_addr  = pmt_from_long((subpkt >> CS_I2CADDR_SHIFT) & 
CS_I2CADDR_MASK);
 
+        // Make a u8 vector to dump the data from the packet into
+        size_t i2c_data_len;
+        pmt_t i2c_data  = pmt_make_u8vector(len - 2, 0);   // skip 
rid+mbz+addr = 2 bytes
+        uint8_t *w_data  = 
+            (uint8_t *) pmt_u8vector_writeable_elements(i2c_data, 
i2c_data_len);
+
+        memcpy(w_data, d_payload + payload_offset + 4, i2c_data_len);  // skip 
first word
+
+        return pmt_list4(s_op_i2c_read_reply, rid, i2c_addr, i2c_data);
+      }
+
       case OP_SPI_READ_REPLY:
         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-04 22:58:57 UTC (rev 5905)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_server.cc 
    2007-07-05 18:40:58 UTC (rev 5906)
@@ -1114,6 +1114,34 @@
                                              pmt_list3(rid, reg_num, 
reg_val))));
     }
 
+    //------------------ I2C READ REPLY -------------------//
+    else if(pmt_eq(op_symbol, s_op_i2c_read_reply)) {
+
+      pmt_t rid       = pmt_nth(1, sub_packet);
+      pmt_t i2c_addr  = pmt_nth(2, sub_packet);
+      pmt_t i2c_data  = pmt_nth(3, sub_packet);
+
+      if(verbose)
+        std::cout << "[USRP_SERVER] Found i2c read reply "
+                  << "("
+                  << "RID: " << rid << ", "
+                  << "Addr: " << i2c_addr
+                  << ")\n";
+
+      // Do some bounds checking to avoid seg faults
+      if(pmt_to_long(rid) > D_OP_I2C_READ_MAX_RID)
+        return;
+
+      pmt_t owner = d_op_i2c_read_owners[pmt_to_long(rid)];
+
+      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_i2c_read_reply,
+                                             pmt_list3(rid, i2c_addr, 
i2c_data))));
+    }
+
     // Each subpacket has an unaccounted for 2 bytes which is the opcode
     // and the length field
     curr_payload += len + 2;

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-04 22:58:57 UTC (rev 5905)
+++ 
gnuradio/branches/developers/gnychis/inband/usrp/host/lib/inband/usrp_tx_stub.cc
    2007-07-05 18:40:58 UTC (rev 5906)
@@ -255,12 +255,19 @@
 
     //---------------- I2C READ ------------------//
     if(pmt_eq(op_symbol, s_op_i2c_read)) {
-      pmt_t rid       = pmt_nth(1, sub_packet);
-      pmt_t i2c_addr  = pmt_nth(2, sub_packet);
-      pmt_t i2c_bytes = pmt_nth(3, sub_packet);
+      long rid       = pmt_to_long(pmt_nth(1, sub_packet));
+      long i2c_addr  = pmt_to_long(pmt_nth(2, sub_packet));
+      long i2c_bytes = pmt_to_long(pmt_nth(3, sub_packet));
 
-      // FIXME: need to generate fake response
+      // Create data to place as a response, filled with 0xff
+      size_t ignore;
+      pmt_t i2c_data = pmt_make_u8vector(i2c_bytes, 0xff);
+      uint8_t *w_data = (uint8_t *) pmt_u8vector_writeable_elements(i2c_data, 
ignore);
 
+      // Generate a reply and put it in the queue for the RX stub to read
+      if(!q_pkt->cs_i2c_read_reply(rid, i2c_addr, w_data, i2c_bytes))
+        goto new_packet;
+
       if(verbose)
         std::cout << "[USRP_TX_STUB] Received i2c read "
                   << "("