Implements USRP2 poke32() command.

git-svn-id: http://gnuradio.org/svn/gnuradio/trunk@10176 221aa14e-8319-0410-a670-987f0aec2ac5

10 files changed, 138 insertions, 1 deletions

diff --git a/gr-usrp2/src/usrp2.i b/gr-usrp2/src/usrp2.i
index 8d3e732ec..e85de2082 100644
--- a/gr-usrp2/src/usrp2.i
+++ b/gr-usrp2/src/usrp2.i
@@ -53,6 +53,7 @@ public:
   bool fpga_master_clock_freq(long *freq);
   bool sync_to_pps();
   std::vector<uint32_t> peek32(uint32_t addr, uint32_t words);
+  bool poke32(uint32_t addr, const std::vector<uint32_t> &data);
 };
 
 // ----------------------------------------------------------------
diff --git a/gr-usrp2/src/usrp2_base.cc b/gr-usrp2/src/usrp2_base.cc
index 1f795421c..9c0d1fa1e 100644
--- a/gr-usrp2/src/usrp2_base.cc
+++ b/gr-usrp2/src/usrp2_base.cc
@@ -74,6 +74,12 @@ usrp2_base::peek32(uint32_t addr, uint32_t words)
 }
 
 bool
+usrp2_base::poke32(uint32_t addr, const std::vector<uint32_t> &data)
+{
+  return d_u2->poke32(addr, data);
+}
+
+bool
 usrp2_base::start()
 {
   // Default implementation is NOP
diff --git a/gr-usrp2/src/usrp2_base.h b/gr-usrp2/src/usrp2_base.h
index ed2a28fe1..4b72a707b 100644
--- a/gr-usrp2/src/usrp2_base.h
+++ b/gr-usrp2/src/usrp2_base.h
@@ -69,6 +69,11 @@ public:
   std::vector<uint32_t> peek32(uint32_t addr, uint32_t words);
 
   /*!
+   * \brief Write memory to Wishbone bus as words
+   */
+  bool poke32(uint32_t addr, const std::vector<uint32_t> &data);
+
+  /*!
    * \brief Called by scheduler when starting flowgraph
    */
   virtual bool start();
diff --git a/usrp2/firmware/apps/app_common_v2.c b/usrp2/firmware/apps/app_common_v2.c
index b51c3b23c..0a51110a9 100644
--- a/usrp2/firmware/apps/app_common_v2.c
+++ b/usrp2/firmware/apps/app_common_v2.c
@@ -350,11 +350,24 @@ peek_cmd(const op_peek_t *p,
   r->rid = p->rid;
   r->ok = true;
 
-  memcpy_wa(reply_payload+sizeof(*r), p->addr, p->bytes);
+  memcpy_wa(reply_payload+sizeof(*r), (void *)p->addr, p->bytes);
 
   return r->len;
 }
 
+static bool
+poke_cmd(const op_poke_t *p)
+{
+  int bytes = p->len - sizeof(*p);
+  putstr("poke: addr="); puthex32(p->addr);
+  printf(" bytes=%u\n", bytes);
+
+  uint8_t *src = (uint8_t *)p + sizeof(*p);
+  memcpy_wa((void *)p->addr, src, bytes);
+
+  return true;
+}
+
 static size_t
 generic_reply(const op_generic_t *p,
 	      void *reply_payload, size_t reply_payload_space,
@@ -463,6 +476,11 @@ handle_control_chan_frame(u2_eth_packet_t *pkt, size_t len)
       subpktlen = peek_cmd((op_peek_t *)payload, reply_payload, reply_payload_space);
       break;
 
+    case OP_POKE:
+      subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
+				poke_cmd((op_poke_t *)payload));
+      break;
+
     default:
       printf("app_common_v2: unhandled opcode = %d\n", gp->opcode);
       break;
diff --git a/usrp2/firmware/include/usrp2_eth_packet.h b/usrp2/firmware/include/usrp2_eth_packet.h
index 8a9994f17..515394fab 100644
--- a/usrp2/firmware/include/usrp2_eth_packet.h
+++ b/usrp2/firmware/include/usrp2_eth_packet.h
@@ -187,6 +187,8 @@ typedef struct {
 #define	OP_SYNC_TO_PPS_REPLY	     (OP_SYNC_TO_PPS | OP_REPLY_BIT)
 #define OP_PEEK                      11
 #define OP_PEEK_REPLY                (OP_PEEK | OP_REPLY_BIT)
+#define OP_POKE                      12
+#define OP_POKE_REPLY                (OP_POKE | OP_REPLY_BIT)
 
 /*
  * All subpackets are a multiple of 4 bytes long.
@@ -388,6 +390,18 @@ typedef struct {
   uint32_t      bytes;
 } _AL4 op_peek_t;
 
+/*!
+ * \brief Write to Wishbone memory
+ */
+typedef struct {
+  uint8_t       opcode;
+  uint8_t	len;
+  uint8_t	rid;
+  uint8_t	mbz;
+  uint32_t      addr;
+  // Words follow here
+} _AL4 op_poke_t;
+
 /*
  * ================================================================
  *             union of all of subpacket types
@@ -406,6 +420,7 @@ typedef union {
   op_config_tx_reply_v2_t	op_config_tx_reply_v2;
   op_config_mimo_t 		op_config_mimo;
   op_peek_t                     op_peek;
+  op_poke_t                     op_poke;
 
 } u2_subpkt_t;
 
diff --git a/usrp2/host/include/usrp2/usrp2.h b/usrp2/host/include/usrp2/usrp2.h
index ed114699c..82b1c6449 100644
--- a/usrp2/host/include/usrp2/usrp2.h
+++ b/usrp2/host/include/usrp2/usrp2.h
@@ -375,6 +375,19 @@ namespace usrp2 {
      */
     std::vector<uint32_t> peek32(uint32_t addr, uint32_t words);
 
+    /*!
+     * Write memory to Wishbone bus as 32-bit words.  Handles endian swapping if needed.
+     *
+     * \param addr      32-bit aligned address.  Only the lower 16-bits are significant
+     * \param data      Vector of 32-bit values to write.
+     *
+     * \returns true iff successful
+     *
+     * WARNING: Attempts to read memory from addresses that do not correspond to RAM or
+     * memory-mapped peripherals may cause the USRP2 to hang, requiring a power cycle.
+     * 
+     */
+    bool poke32(uint32_t addr, const std::vector<uint32_t> &data);
 
 #if 0	// not yet implemented
     /*!
diff --git a/usrp2/host/lib/control.h b/usrp2/host/lib/control.h
index 774ca85f6..2c042f0cf 100644
--- a/usrp2/host/lib/control.h
+++ b/usrp2/host/lib/control.h
@@ -86,6 +86,13 @@ namespace usrp2 {
     op_generic_t    eop;
   };
 
+  struct op_poke_cmd {
+    u2_eth_packet_t h;
+    op_poke_t       op;
+    // words to write go here
+    // eop must be dynamically written here
+  };
+
   /*!
    * Control mechanism to allow API calls to block waiting for reply packets
    */    
diff --git a/usrp2/host/lib/usrp2.cc b/usrp2/host/lib/usrp2.cc
index 37d8aaa33..2a7fe5963 100644
--- a/usrp2/host/lib/usrp2.cc
+++ b/usrp2/host/lib/usrp2.cc
@@ -401,6 +401,12 @@ namespace usrp2 {
     return d_impl->peek32(addr, words);
   }
 
+  bool
+  usrp2::poke32(uint32_t addr, const std::vector<uint32_t> &data)
+  {
+    return d_impl->poke32(addr, data);
+  }
+
 } // namespace usrp2
 
 
diff --git a/usrp2/host/lib/usrp2_impl.cc b/usrp2/host/lib/usrp2_impl.cc
index c44c65981..04f50f480 100644
--- a/usrp2/host/lib/usrp2_impl.cc
+++ b/usrp2/host/lib/usrp2_impl.cc
@@ -1084,4 +1084,67 @@ namespace usrp2 {
     return result;
   }
 
+  bool
+  usrp2::impl::poke32(uint32_t addr, const std::vector<uint32_t> &data)
+  {
+    if (addr % 4 != 0) {
+      fprintf(stderr, "usrp2::poke32: addr (=%08X) must be 32-bit word aligned\n", addr); 
+      return false;
+    }
+
+    int plen = sizeof(op_poke_cmd);
+    int wlen = sizeof(uint32_t);
+    int max_words = (MAX_SUBPKT_LEN-plen)/wlen;
+    int words = data.size();
+
+    if (words > max_words) {
+      fprintf(stderr, "usrp2::poke32: write size (=%u) exceeds maximum of %u words\n",
+	      words, max_words);
+      return false;
+    }
+
+    //fprintf(stderr, "usrp2::poke32: addr=%08X words=%u\n", addr, words);
+
+    if (words == 0)
+      return true; // NOP
+
+    op_poke_cmd  *cmd;
+    op_generic_t *eop;
+
+    // Allocate, clear, and initialize command packet
+    int bytes = words*wlen;
+    int l = plen+bytes+sizeof(*eop); // op_poke_cmd+data+eop
+    cmd = (op_poke_cmd *)malloc(l);
+    //fprintf(stderr, "cmd=%p l=%i\n", cmd, l);
+    memset(cmd, 0, l);
+    init_etf_hdrs(&cmd->h, d_addr, 0, CONTROL_CHAN, -1);
+    cmd->op.opcode = OP_POKE;
+    cmd->op.len = sizeof(cmd->op)+bytes;
+    cmd->op.rid = d_next_rid++;
+    cmd->op.addr = htonl(addr);
+
+    // Copy data from vector into packet space
+    uint32_t *dest = (uint32_t *)((uint8_t *)cmd+plen);
+    for (unsigned int i = 0; i < words; i++) {
+      //fprintf(stderr, "%03i@%p\n", i, dest);
+      *dest++ = htonl(data[i]);
+    }
+
+    // Write end-of-packet subpacket
+    eop = (op_generic_t *)dest;
+    eop->opcode = OP_EOP;
+    eop->len = sizeof(*eop);
+    //fprintf(stderr, "eop=%p len=%i\n", eop, eop->len);
+
+    // Send command to device and retrieve reply
+    bool ok = false;
+    op_generic_t reply;
+    pending_reply p(cmd->op.rid, &reply, sizeof(reply));
+    if (transmit_cmd(cmd, l, &p, DEF_CMD_TIMEOUT))
+      ok = (ntohx(reply.ok) == 1);
+
+    free(cmd);
+    return ok;
+  }
+
 } // namespace usrp2
diff --git a/usrp2/host/lib/usrp2_impl.h b/usrp2/host/lib/usrp2_impl.h
index 0a6b97b86..0400a108b 100644
--- a/usrp2/host/lib/usrp2_impl.h
+++ b/usrp2/host/lib/usrp2_impl.h
@@ -27,6 +27,8 @@
 #include "ring.h"
 #include <string>
 
+#define MAX_SUBPKT_LEN 252
+
 namespace usrp2 {
   
   class eth_buffer;
@@ -175,6 +177,7 @@ namespace usrp2 {
     bool burn_mac_addr(const std::string &new_addr);
     bool sync_to_pps();
     std::vector<uint32_t> peek32(uint32_t addr, uint32_t words);
+    bool poke32(uint32_t addr, const std::vector<uint32_t> &data);
   };
   
 } // namespace usrp2