Merged r10712:10765 from jcorgan/gpio into trunk. Adds out-of-band and streaming GPIO functions for USRP2.

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

18 files changed, 649 insertions, 49 deletions

diff --git a/gr-usrp2/src/usrp2.i b/gr-usrp2/src/usrp2.i
index 216aec937..3d6da0606 100644
--- a/gr-usrp2/src/usrp2.i
+++ b/gr-usrp2/src/usrp2.i
@@ -81,6 +81,12 @@ public:
   bool daughterboard_id(int *dbid);
   unsigned int overruns();
   unsigned int missing();
+  bool set_gpio_ddr(uint16_t value, uint16_t mask);
+  bool set_gpio_sels(std::string sels);
+  bool write_gpio(uint16_t value, uint16_t mask);
+  %rename(_real_read_gpio) read_gpio;
+  bool read_gpio(uint16_t *value);
+  bool enable_gpio_streaming(int enable);
 };
 
 // ----------------------------------------------------------------
@@ -147,6 +153,11 @@ public:
   double freq_max();
   %rename(_real_daughterboard_id) daughterboard_id;
   bool daughterboard_id(int *dbid);
+  bool set_gpio_ddr(uint16_t value, uint16_t mask);
+  bool set_gpio_sels(std::string sels);
+  bool write_gpio(uint16_t value, uint16_t mask);
+  %rename(_real_read_gpio) read_gpio;
+  bool read_gpio(uint16_t *value);
 };
 
 // ----------------------------------------------------------------
@@ -189,12 +200,15 @@ public:
 
 // some utility functions to allow Python to deal with pointers
 %{
-  long *make_long_ptr() { return (long *)malloc(sizeof(long)); }
+  long *make_long_ptr() { return new long; }
   long deref_long_ptr(long *l) { return *l; }
-  void free_long_ptr(long *l) { free(l); }
-  int *make_int_ptr() { return (int *)malloc(sizeof(int)); }
+  void free_long_ptr(long *l) { delete l; }
+  int *make_int_ptr() { return new int; }
   int deref_int_ptr(int *l) { return *l; }
-  void free_int_ptr(int *l) { free(l); }
+  void free_int_ptr(int *l) { delete l; }
+  uint16_t *make_uint16_ptr() { return new uint16_t; }
+  int deref_uint16_ptr(uint16_t *l) { return *l; }
+  void free_uint16_ptr(uint16_t *l) { delete l; }
 %}
 
 long *make_long_ptr();
@@ -203,6 +217,9 @@ void free_long_ptr(long *l);
 int *make_int_ptr();
 int deref_int_ptr(int *l);
 void free_int_ptr(int *l);
+uint16_t *make_uint16_ptr();
+int deref_uint16_ptr(uint16_t *l);
+void free_uint16_ptr(uint16_t *l);
 
 // create a more pythonic interface
 %pythoncode %{
@@ -273,6 +290,17 @@ def __default_tx_scale_iq(self, interp):
   self._real_default_tx_scale_iq(interp, scale_i, scale_q)
   return (deref_int_ptr(scale_i), deref_int_ptr(scale_q))
 
+def __read_gpio(self):
+  value = make_uint16_ptr()
+  r = self._real_read_gpio(value)
+  if r:
+    result = deref_uint16_ptr(value)
+  else:
+    result = None
+  free_uint16_ptr(value)
+  return result
+
+
 usrp2_source_32fc_sptr.set_center_freq = __set_center_freq
 usrp2_source_16sc_sptr.set_center_freq = __set_center_freq
 usrp2_sink_32fc_sptr.set_center_freq = __set_center_freq
@@ -306,4 +334,9 @@ usrp2_sink_16sc_sptr.daughterboard_id = __daughterboard_id
 usrp2_sink_32fc_sptr.default_scale_iq = __default_tx_scale_iq
 usrp2_sink_16sc_sptr.default_scale_iq = __default_tx_scale_iq
 
+usrp2_source_32fc_sptr.read_gpio = __read_gpio
+usrp2_source_16sc_sptr.read_gpio = __read_gpio
+usrp2_sink_32fc_sptr.read_gpio = __read_gpio
+usrp2_sink_16sc_sptr.read_gpio = __read_gpio
+
 %}
diff --git a/gr-usrp2/src/usrp2_sink_base.cc b/gr-usrp2/src/usrp2_sink_base.cc
index 8118407c5..4579d1651 100644
--- a/gr-usrp2/src/usrp2_sink_base.cc
+++ b/gr-usrp2/src/usrp2_sink_base.cc
@@ -129,3 +129,23 @@ usrp2_sink_base::daughterboard_id(int *dbid)
 {
   return d_u2->tx_daughterboard_id(dbid);
 }
+
+bool usrp2_sink_base::set_gpio_ddr(uint16_t value, uint16_t mask)
+{
+  return d_u2->set_gpio_ddr(usrp2::GPIO_TX_BANK, value, mask);
+}
+
+bool usrp2_sink_base::set_gpio_sels(std::string sels)
+{
+  return d_u2->set_gpio_sels(usrp2::GPIO_TX_BANK, sels);
+}
+
+bool usrp2_sink_base::write_gpio(uint16_t value, uint16_t mask)
+{
+  return d_u2->write_gpio(usrp2::GPIO_TX_BANK, value, mask);
+}
+
+bool usrp2_sink_base::read_gpio(uint16_t *value)
+{
+  return d_u2->read_gpio(usrp2::GPIO_TX_BANK, value);
+}
diff --git a/gr-usrp2/src/usrp2_sink_base.h b/gr-usrp2/src/usrp2_sink_base.h
index 37905f4e8..f973e805c 100644
--- a/gr-usrp2/src/usrp2_sink_base.h
+++ b/gr-usrp2/src/usrp2_sink_base.h
@@ -114,6 +114,26 @@ public:
    * -2 if invalid EEPROM on daughterboard.
    */
   bool daughterboard_id(int *dbid);
+
+  /*!
+   * \brief Set daughterboard GPIO data direction register.
+   */
+  bool set_gpio_ddr(uint16_t value, uint16_t mask);
+
+  /*!
+   * \brief Set daughterboard GPIO output selection register.
+   */
+  bool set_gpio_sels(std::string sels);
+
+  /*!
+   * \brief Set daughterboard GPIO pin values.
+   */
+  bool write_gpio(uint16_t value, uint16_t mask);
+
+  /*!
+   * \brief Read daughterboard GPIO pin values
+   */
+  bool read_gpio(uint16_t *value);
 };
 
 #endif /* INCLUDED_USRP2_SINK_BASE_H */
diff --git a/gr-usrp2/src/usrp2_source_base.cc b/gr-usrp2/src/usrp2_source_base.cc
index 8bcac5d69..0ad7008a6 100644
--- a/gr-usrp2/src/usrp2_source_base.cc
+++ b/gr-usrp2/src/usrp2_source_base.cc
@@ -147,3 +147,33 @@ usrp2_source_base::stop()
 {
   return d_u2->stop_rx_streaming(0); // FIXME: someday sources will have channel #s
 }
+
+bool
+usrp2_source_base::set_gpio_ddr(uint16_t value, uint16_t mask)
+{
+  return d_u2->set_gpio_ddr(usrp2::GPIO_RX_BANK, value, mask);
+}
+
+bool
+usrp2_source_base::set_gpio_sels(std::string sels)
+{
+  return d_u2->set_gpio_sels(usrp2::GPIO_RX_BANK, sels);
+}
+
+bool
+usrp2_source_base::write_gpio(uint16_t value, uint16_t mask)
+{
+  return d_u2->write_gpio(usrp2::GPIO_RX_BANK, value, mask);
+}
+
+bool
+usrp2_source_base::read_gpio(uint16_t *value)
+{
+  return d_u2->read_gpio(usrp2::GPIO_RX_BANK, value);
+}
+
+bool
+usrp2_source_base::enable_gpio_streaming(int enable)
+{
+  return d_u2->enable_gpio_streaming(usrp2::GPIO_RX_BANK, enable);
+}
diff --git a/gr-usrp2/src/usrp2_source_base.h b/gr-usrp2/src/usrp2_source_base.h
index f98d329fd..2e2d51fc3 100644
--- a/gr-usrp2/src/usrp2_source_base.h
+++ b/gr-usrp2/src/usrp2_source_base.h
@@ -129,6 +129,31 @@ public:
    * \brief Called by scheduler when stopping flowgraph
    */
   virtual bool stop();
+
+  /*!
+   * \brief Set daughterboard GPIO data direction register.
+   */
+  bool set_gpio_ddr(uint16_t value, uint16_t mask);
+
+  /*!
+   * \brief Set daughterboard GPIO output selection register.
+   */
+  bool set_gpio_sels(std::string sels);
+
+  /*!
+   * \brief Set daughterboard GPIO pin values.
+   */
+  bool write_gpio(uint16_t value, uint16_t mask);
+
+  /*!
+   * \brief Read daughterboard GPIO pin values
+   */
+  bool read_gpio(uint16_t *value);
+
+  /*!
+   * \brief Enable streaming GPIO in sample LSBs
+   */
+  bool enable_gpio_streaming(int enable);
 };
 
 #endif /* INCLUDED_USRP2_SOURCE_BASE_H */
diff --git a/usrp2/firmware/apps/app_common_v2.c b/usrp2/firmware/apps/app_common_v2.c
index 60fa63ee3..6d9606d45 100644
--- a/usrp2/firmware/apps/app_common_v2.c
+++ b/usrp2/firmware/apps/app_common_v2.c
@@ -40,17 +40,11 @@ dbsm_t *ac_could_be_sending_to_eth;
 
 static unsigned char exp_seqno __attribute__((unused)) = 0;
 
-static bool
-burn_mac_addr(const op_burn_mac_addr_t *p)
-{
-  return ethernet_set_mac_addr(&p->addr);
-}
-
-static bool
+static inline bool
 sync_to_pps(const op_generic_t *p)
 {
   timesync_regs->sync_on_next_pps = 1;
-  putstr("SYNC to PPS\n");
+  //putstr("SYNC to PPS\n");
   return true;
 }
 
@@ -65,7 +59,7 @@ sync_every_pps(const op_generic_t *p)
   return true;
 }
 
-static bool
+static inline bool
 config_mimo_cmd(const op_config_mimo_t *p)
 {
   clocks_mimo_config(p->flags);
@@ -348,8 +342,8 @@ peek_cmd(const op_peek_t *p,
 {
   op_generic_t *r = (op_generic_t *) reply_payload;
 
-  putstr("peek: addr="); puthex32(p->addr);
-  printf(" bytes=%u\n", p->bytes);
+  //putstr("peek: addr="); puthex32(p->addr);
+  //printf(" bytes=%u\n", p->bytes);
 
   if ((reply_payload_space < (sizeof(*r) + p->bytes)) ||
       p->bytes > MAX_SUBPKT_LEN - sizeof(op_generic_t)) {
@@ -371,8 +365,8 @@ 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);
+  //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);
@@ -391,6 +385,26 @@ set_lo_offset_cmd(const op_freq_t *p)
 }
 
 static size_t
+gpio_read_cmd(const op_gpio_t *p,
+	      void *reply_payload, size_t reply_payload_space)
+{
+  op_gpio_read_reply_t *r = (op_gpio_read_reply_t *) reply_payload;
+  if (reply_payload_space < sizeof(*r))	// no room
+    return 0;
+
+ // Build reply subpacket
+
+  r->opcode = OP_GPIO_READ_REPLY;
+  r->len = sizeof(op_gpio_read_reply_t);
+  r->rid = p->rid;
+  r->ok = true;
+  r->mbz = 0;
+  r->value = hal_gpio_read(p->bank);
+
+  return r->len;
+}
+
+static size_t
 generic_reply(const op_generic_t *p,
 	      void *reply_payload, size_t reply_payload_space,
 	      bool ok)
@@ -438,6 +452,7 @@ handle_control_chan_frame(u2_eth_packet_t *pkt, size_t len)
   int payload_len = len - sizeof(u2_eth_packet_t);
   
   size_t subpktlen = 0;
+  bool ok = false;
 
   while (payload_len >= sizeof(op_generic_t)){
     const op_generic_t *gp = (const op_generic_t *) payload;
@@ -454,34 +469,30 @@ handle_control_chan_frame(u2_eth_packet_t *pkt, size_t len)
       break;
     
     case OP_CONFIG_TX_V2:
-      subpktlen = config_tx_v2_cmd((op_config_tx_v2_t *) payload,
-				   reply_payload, reply_payload_space);
+      subpktlen = config_tx_v2_cmd((op_config_tx_v2_t *) payload, reply_payload, reply_payload_space);
       break;
 
     case OP_CONFIG_RX_V2:
-      subpktlen = config_rx_v2_cmd((op_config_rx_v2_t *) payload,
-				   reply_payload, reply_payload_space);
+      subpktlen = config_rx_v2_cmd((op_config_rx_v2_t *) payload, reply_payload, reply_payload_space);
       break;
 
     case OP_START_RX_STREAMING:
       start_rx_streaming_cmd(&pkt->ehdr.src, (op_start_rx_streaming_t *) payload);
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space, true);
-      break;
+      ok = true;
+      goto generic_reply;
     
     case OP_STOP_RX:
       stop_rx_cmd();
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space, true);
-      break;
+      ok = true;
+      goto generic_reply;
     
     case OP_BURN_MAC_ADDR:
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
-				burn_mac_addr((op_burn_mac_addr_t *) payload));
-      break;
+      ok = ethernet_set_mac_addr(&((op_burn_mac_addr_t *)payload)->addr);
+      goto generic_reply;
 
     case OP_CONFIG_MIMO:
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
-				config_mimo_cmd((op_config_mimo_t *) payload));
-      break;
+      ok = config_mimo_cmd((op_config_mimo_t *) payload);
+      goto generic_reply;
 
     case OP_READ_TIME:
       subpktlen = read_time_cmd(gp, reply_payload, reply_payload_space);
@@ -492,33 +503,65 @@ handle_control_chan_frame(u2_eth_packet_t *pkt, size_t len)
       break;
 
     case OP_SYNC_TO_PPS:
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
-				sync_to_pps((op_generic_t *) payload));
-      break;
+      sync_to_pps((op_generic_t *) payload);
+      ok = true;
+      goto generic_reply;
 
     case OP_PEEK:
       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;
+      ok = poke_cmd((op_poke_t *)payload);
+      goto generic_reply;
 
     case OP_SET_TX_LO_OFFSET:
     case OP_SET_RX_LO_OFFSET:
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
-				set_lo_offset_cmd((op_freq_t *)payload));
-      break;
+      ok = set_lo_offset_cmd((op_freq_t *)payload);
+      goto generic_reply;
 
     case OP_RESET_DB:
       db_init();
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space, true);
-      break;
+      ok = true;
+      goto generic_reply;
 
     case OP_SYNC_EVERY_PPS:
-      subpktlen = generic_reply(gp, reply_payload, reply_payload_space,
-				sync_every_pps((op_generic_t *) payload));
+      ok = sync_every_pps((op_generic_t *) payload);
+      goto generic_reply;
+
+    case OP_GPIO_SET_DDR:
+      ok = true;
+      hal_gpio_set_ddr(((op_gpio_t *)payload)->bank, 
+		       ((op_gpio_t *)payload)->value, 
+		       ((op_gpio_t *)payload)->mask);
+      goto generic_reply;
+
+    case OP_GPIO_SET_SELS:
+      ok = true;
+      hal_gpio_set_sels(((op_gpio_set_sels_t *)payload)->bank,
+			(char *)(&((op_gpio_set_sels_t *)payload)->sels));
+      goto generic_reply;
+
+    case OP_GPIO_READ:
+      subpktlen = gpio_read_cmd((op_gpio_t *) payload, reply_payload, reply_payload_space);
+      break;
+
+    case OP_GPIO_WRITE:
+      ok = true;
+      hal_gpio_write(((op_gpio_t *)payload)->bank, 
+		     ((op_gpio_t *)payload)->value, 
+		     ((op_gpio_t *)payload)->mask);
+      goto generic_reply;
+
+    case OP_GPIO_STREAM:
+      ok = true;
+      dsp_rx_regs->gpio_stream_enable = (uint32_t)((op_gpio_t *)payload)->value;
+      goto generic_reply;
+
+    // Add new opcode handlers here
+
+    generic_reply:
+      subpktlen = generic_reply(gp, reply_payload, reply_payload_space, ok);
       break;
 
     default:
diff --git a/usrp2/firmware/include/usrp2_eth_packet.h b/usrp2/firmware/include/usrp2_eth_packet.h
index a118c1b55..b0123c584 100644
--- a/usrp2/firmware/include/usrp2_eth_packet.h
+++ b/usrp2/firmware/include/usrp2_eth_packet.h
@@ -198,6 +198,16 @@ typedef struct {
 #define OP_RESET_DB_REPLY            (OP_RESET_DB | OP_REPLY_BIT)
 #define OP_SYNC_EVERY_PPS            16
 #define OP_SYNC_EVERY_PPS_REPLY      (OP_SYNC_EVERY_PPS | OP_REPLY_BIT)
+#define OP_GPIO_SET_DDR              17
+#define OP_GPIO_SET_DDR_REPLY        (OP_GPIO_SET_DDR | OP_REPLY_BIT)
+#define OP_GPIO_SET_SELS             18
+#define OP_GPIO_SET_SELS_REPLY       (OP_GPIO_SET_SELS | OP_REPLY_BIT)
+#define OP_GPIO_READ                 19
+#define OP_GPIO_READ_REPLY           (OP_GPIO_READ | OP_REPLY_BIT)
+#define OP_GPIO_WRITE                20
+#define OP_GPIO_WRITE_REPLY          (OP_GPIO_WRITE | OP_REPLY_BIT)
+#define OP_GPIO_STREAM               21
+#define OP_GPIO_STREAM_REPLY         (OP_GPIO_STREAM | OP_REPLY_BIT)
 
 /*
  * All subpackets are a multiple of 4 bytes long.
@@ -425,6 +435,35 @@ typedef struct {
 } _AL4 op_freq_t;
 
 /*
+ * Structures for commands in GPIO system
+ */
+typedef struct {
+  uint8_t       opcode;		// OP_GPIO_SET_DDR, OP_GPIO_WRITE, OP_GPIO_STREAM
+  uint8_t       len;
+  uint8_t       rid;
+  uint8_t       bank;
+  uint16_t      value;
+  uint16_t      mask;
+} _AL4 op_gpio_t;
+
+typedef struct {
+  uint8_t       opcode;		// OP_GPIO_SET_SELS
+  uint8_t       len;
+  uint8_t       rid;
+  uint8_t       bank;
+  uint8_t       sels[16];
+} _AL4 op_gpio_set_sels_t;
+
+typedef struct {
+  uint8_t	opcode;		// OP_GPIO_READ_REPLY
+  uint8_t	len;
+  uint8_t	rid;
+  uint8_t	ok;
+  uint16_t      mbz;
+  uint16_t	value;
+} _AL4 op_gpio_read_reply_t;
+
+/*
  * ================================================================
  *             union of all of subpacket types
  * ================================================================
@@ -444,6 +483,9 @@ typedef union {
   op_peek_t                     op_peek;
   op_poke_t                     op_poke;
   op_freq_t                     op_freq;
+  op_gpio_t                     op_gpio;
+  op_gpio_set_sels_t            op_gpio_set_sels;
+  op_gpio_read_reply_t          op_gpio_read_reply;
 
 } u2_subpkt_t;
 
diff --git a/usrp2/firmware/lib/memory_map.h b/usrp2/firmware/lib/memory_map.h
index fb6fc45d7..78a4330d2 100644
--- a/usrp2/firmware/lib/memory_map.h
+++ b/usrp2/firmware/lib/memory_map.h
@@ -471,6 +471,31 @@ typedef struct {
    */
   volatile uint32_t     rx_mux;        // called adc_mux in dsp_core_rx.v
 
+  /*!
+   * \brief Streaming GPIO configuration
+   *
+   * This determines whether the LSBs of I and Q samples come from the DSP
+   * pipeline or from the io_rx GPIO pins.  To stream GPIO, one must first
+   * set the GPIO data direction register to have io_rx[15] and/or io_rx[14]
+   * configured as inputs.  The GPIO pins will be sampled at the time the
+   * remainder of the DSP sample is strobed into the RX sample FIFO.  There
+   * will be a decimation-dependent fixed time offset between the GPIO
+   * sample stream and the associated RF samples.
+   *
+   *    3                   2                   1                       
+   *  1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+   * +-------+-------+-------+-------+-------+-------+-------+-------+
+   * |                           MBZ                             |Q|I|
+   * +-------+-------+-------+-------+-------+-------+-------+-------+
+   *
+   * I         0=LSB comes from DSP pipeline (default)
+   *           1=LSB comes from io_rx[15]
+   * 
+   * Q         0=LSB comes from DSP pipeline (default)
+   *           1=LSB comes from io_rx[14]
+   */
+  volatile uint32_t gpio_stream_enable;
+
 } dsp_rx_regs_t;
   
 #define dsp_rx_regs ((dsp_rx_regs_t *) DSP_RX_BASE)
diff --git a/usrp2/firmware/lib/u2_init.c b/usrp2/firmware/lib/u2_init.c
index 948055694..713b2c321 100644
--- a/usrp2/firmware/lib/u2_init.c
+++ b/usrp2/firmware/lib/u2_init.c
@@ -51,13 +51,15 @@ get_hw_rev(void)
 bool
 u2_init(void)
 {
-  // Set GPIOs to inputs
+  // Set GPIOs to inputs, disable GPIO streaming
   hal_gpio_set_ddr(GPIO_TX_BANK, 0x0000, 0xffff);
   hal_gpio_set_ddr(GPIO_RX_BANK, 0x0000, 0xffff);
 
   hal_gpio_write(GPIO_TX_BANK, 0x0000, 0xffff);	// init s/w output value to zero
   hal_gpio_write(GPIO_RX_BANK, 0x0000, 0xffff);
 
+  dsp_rx_regs->gpio_stream_enable = 0; // I, Q LSBs come from DSP
+
   hal_io_init();
 
   // init spi, so that we can switch over to the high-speed clock
diff --git a/usrp2/fpga/sdr_lib/dsp_core_rx.v b/usrp2/fpga/sdr_lib/dsp_core_rx.v
index 716182f65..ee713e4ac 100644
--- a/usrp2/fpga/sdr_lib/dsp_core_rx.v
+++ b/usrp2/fpga/sdr_lib/dsp_core_rx.v
@@ -7,6 +7,8 @@ module dsp_core_rx
    input [13:0] adc_a, input adc_ovf_a,
    input [13:0] adc_b, input adc_ovf_b,
    
+   input [15:0] io_rx,
+
    output [31:0] sample,
    input run,
    output strobe,
@@ -56,6 +58,11 @@ module dsp_core_rx
      (.clk(clk),.rst(rst),.strobe(set_stb),.addr(set_addr),
       .in(set_data),.out(muxctrl),.changed());
 
+   wire [1:0] gpio_ena;
+   setting_reg #(.my_addr(`DSP_CORE_RX_BASE+9)) sr_9
+     (.clk(clk),.rst(rst),.strobe(set_stb),.addr(set_addr),
+      .in(set_data),.out(gpio_ena),.changed());
+
    // The TVRX connects to what is called adc_b, thus A and B are
    // swapped throughout the design.
    //
@@ -151,7 +158,21 @@ module dsp_core_rx
    round #(.bits_in(18),.bits_out(16)) round_iout (.in(i_hb2),.out(i_out));
    round #(.bits_in(18),.bits_out(16)) round_qout (.in(q_hb2),.out(q_out));
 
-   assign      sample = {i_out,q_out};
+   // Streaming GPIO
+   //
+   // io_rx[15] => I channel LSB if gpio_ena[0] high
+   // io_rx[14] => Q channel LSB if gpio_ena[1] high
+
+   reg [31:0] sample_reg;
+   always @(posedge clk)
+     begin
+	sample_reg[31:17] <= i_out[15:1];
+	sample_reg[15:1]  <= q_out[15:1];
+	sample_reg[16]    <= gpio_ena[0] ? io_rx[15] : i_out[0]; 
+	sample_reg[0]     <= gpio_ena[1] ? io_rx[14] : q_out[0];
+     end
+   
+   assign      sample = sample_reg;
    assign      strobe = strobe_hb2;
    assign      debug = {enable_hb1, enable_hb2, run, strobe, strobe_cic, strobe_cic_d1, strobe_hb1, strobe_hb2};
    
diff --git a/usrp2/fpga/top/u2_core/u2_core.v b/usrp2/fpga/top/u2_core/u2_core.v
index a2b52401e..f12b5af4d 100755
--- a/usrp2/fpga/top/u2_core/u2_core.v
+++ b/usrp2/fpga/top/u2_core/u2_core.v
@@ -602,7 +602,7 @@ module u2_core
      (.clk(dsp_clk),.rst(dsp_rst),
       .set_stb(set_stb),.set_addr(set_addr),.set_data(set_data),
       .adc_a(adc_a),.adc_ovf_a(adc_ovf_a),.adc_b(adc_b),.adc_ovf_b(adc_ovf_b),
-      .sample(sample_rx), .run(run_rx_d1), .strobe(strobe_rx),
+      .io_rx(io_rx),.sample(sample_rx), .run(run_rx_d1), .strobe(strobe_rx),
       .debug(debug_rx_dsp) );
 
    tx_control #(.FIFOSIZE(10)) tx_control
diff --git a/usrp2/host/apps/Makefile.am b/usrp2/host/apps/Makefile.am
index b6f1c03ff..da44776c9 100644
--- a/usrp2/host/apps/Makefile.am
+++ b/usrp2/host/apps/Makefile.am
@@ -36,7 +36,8 @@ noinst_PROGRAMS = \
 	gen_const \
 	rx_streaming_samples \
 	tx_samples \
-	test_mimo_tx
+	test_mimo_tx \
+	gpio
 
 find_usrps_SOURCES = find_usrps.cc
 usrp2_burn_mac_addr_SOURCES = usrp2_burn_mac_addr.cc
diff --git a/usrp2/host/apps/gpio.cc b/usrp2/host/apps/gpio.cc
new file mode 100644
index 000000000..770b54ee2
--- /dev/null
+++ b/usrp2/host/apps/gpio.cc
@@ -0,0 +1,46 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2009 Free Software Foundation, Inc.
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include <usrp2/usrp2.h>
+#include <iostream>
+
+int
+main(int argc, char **argv)
+{
+  usrp2::usrp2::sptr u2 = usrp2::usrp2::make("eth0");
+
+  // Set io_tx[15] as FPGA output
+  u2->set_gpio_ddr(usrp2::GPIO_TX_BANK, 0x8000, 0x8000);
+
+  // Set io_tx[15] under host sofware control
+  u2->set_gpio_sels(usrp2::GPIO_TX_BANK, "s...............");
+
+  // Write pins
+  uint16_t v = 0x8765;
+  u2->write_gpio(usrp2::GPIO_TX_BANK, v, 0x8000);
+
+  // Read back
+  v = 0;
+  u2->read_gpio(usrp2::GPIO_TX_BANK, &v);
+  printf("TX GPIO read: %04X\n", v);
+  return 0;
+}
diff --git a/usrp2/host/include/usrp2/usrp2.h b/usrp2/host/include/usrp2/usrp2.h
index 70d800bf0..e80eda3e4 100644
--- a/usrp2/host/include/usrp2/usrp2.h
+++ b/usrp2/host/include/usrp2/usrp2.h
@@ -63,6 +63,10 @@ namespace usrp2 {
   props_vector_t find(const std::string &ifc, const std::string &mac_addr=""); 
 
   class tune_result;
+
+  // FIXME: get from firmware include
+  static const int GPIO_TX_BANK = 0;
+  static const int GPIO_RX_BANK = 1;
   
   class usrp2 : boost::noncopyable
   {
@@ -414,6 +418,91 @@ namespace usrp2 {
      */
     bool poke32(uint32_t addr, const std::vector<uint32_t> &data);
 
+    /*!
+     * Set daughterboard GPIO data direction register.
+     *
+     * \param bank      GPIO_TX_BANK or GPIO_RX_BANK
+     * \param value     16-bits, 0=FPGA input, 1=FPGA output
+     * \param mask      16-bits, 0=ignore, 1=set
+     *
+     * \returns true iff successful
+     *
+     * WARNING: Improper usage of this function may result in damage to the USRP2
+     *
+     */
+    bool set_gpio_ddr(int bank, uint16_t value, uint16_t mask);
+
+    /*!
+     * Set daughterboard GPIO output selection register.  For those GPIO pins that
+     * are configured as outputs in the DDR, this settings configures the source
+     * of the pin value.
+     *
+     * \param bank      GPIO_TX_BANK or GPIO_RX_BANK
+     * \param sels      Exactly 16 character MSB->LSB string. For each position:
+     *                    '.' = ignore this bit, i.e., leave current value
+     *                    'a' = Output ATR value
+     *                    's' = Output host software controlled value
+     *                    '0' = Output FPGA debug bus 0 value
+     *                    '1' = Output FPGA debug bus 1 value
+     *
+     * \returns true iff successful
+     *
+     * WARNING: Improper usage of this function may result in damage to the USRP2
+     *
+     */
+    bool set_gpio_sels(int bank, std::string sels);
+
+    /*!
+     * Set daughterboard GPIO pin values.
+     *
+     * \param bank     GPIO_TX_BANK or GPIO_RX_BANK
+     * \param value    16 bits, 0=low, 1=high
+     * \param mask     16 bits, 0=ignore, 1=set
+     *
+     * \returns true iff successful
+     *
+     * WARNING: Improper usage of this function may result in damage to the USRP2
+     *
+     */
+    bool write_gpio(int bank, uint16_t value, uint16_t mask);
+
+    /*!
+     * Read daughterboard GPIO pin values
+     *
+     * \param bank     GPIO_TX_BANK or GPIO_RX_BANK
+     * \param value    pointer to uint16_t to hold read results
+     *
+     * \returns true iff successful
+     *
+     */
+    bool read_gpio(int bank, uint16_t *value);
+
+    /*!
+     * Set GPIO streaming mode
+     *
+     * Individually enables streaming GPIO pins through LSBs of DSP
+     * samples.
+     *
+     * On receive, io_rx[15] replaces I[0], io_rx[14] replaces Q[0]
+     * On transmit, I[0] maps to io_tx[15], Q[0] maps to io_tx[14]
+     * (Transmit streaming is not yet implemented.)
+     *
+     * The selected GPIO pins must have been set as inputs or outputs
+     * and, for transmit, set to software control.
+     *
+     * When enabled, the replaced DSP sample LSBs become 0.
+     *
+     * \param bank     GPIO_TX_BANK or GPIO_RX_BANK
+     * \param enable   enable[0] controls I channel LSB
+     *                 enable[1] controls Q channel LSB
+     *
+     * \returns true iff successful
+     *
+     * WARNING: Improper usage of this function may result in damage to the USRP2
+     *
+     */
+    bool enable_gpio_streaming(int bank, int enable);
+
 #if 0	// not yet implemented
     /*!
      * \brief Write EEPROM on motherboard or any daughterboard.
diff --git a/usrp2/host/lib/control.h b/usrp2/host/lib/control.h
index 0e8fcfe57..9adc1618f 100644
--- a/usrp2/host/lib/control.h
+++ b/usrp2/host/lib/control.h
@@ -99,6 +99,18 @@ namespace usrp2 {
     op_generic_t    eop;
   };
 
+  struct op_gpio_cmd {
+    u2_eth_packet_t h;
+    op_gpio_t       op;
+    op_generic_t    eop;
+  };
+
+  struct op_gpio_set_sels_cmd {
+    u2_eth_packet_t h;
+    op_gpio_set_sels_t op;
+    op_generic_t    eop;
+  };
+
   /*!
    * 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 2e8540e91..8160d01fa 100644
--- a/usrp2/host/lib/usrp2.cc
+++ b/usrp2/host/lib/usrp2.cc
@@ -437,8 +437,37 @@ namespace usrp2 {
     return d_impl->poke32(addr, data);
   }
 
-} // namespace usrp2
+  bool
+  usrp2::set_gpio_ddr(int bank, uint16_t value, uint16_t mask)
+  {
+    return d_impl->set_gpio_ddr(bank, value, mask);
+  }
 
+  bool
+  usrp2::set_gpio_sels(int bank, std::string src)
+  {
+    return d_impl->set_gpio_sels(bank, src);
+  }
+
+  bool
+  usrp2::write_gpio(int bank, uint16_t value, uint16_t mask)
+  {
+    return d_impl->write_gpio(bank, value, mask);
+  }
+
+  bool
+  usrp2::read_gpio(int bank, uint16_t *value)
+  {
+    return d_impl->read_gpio(bank, value);
+  }
+
+  bool
+  usrp2::enable_gpio_streaming(int bank, int enable)
+  {
+    return d_impl->enable_gpio_streaming(bank, enable);
+  }
+
+} // namespace usrp2
 
 std::ostream& operator<<(std::ostream &os, const usrp2::props &x)
 {
diff --git a/usrp2/host/lib/usrp2_impl.cc b/usrp2/host/lib/usrp2_impl.cc
index 9c30bce92..561b53101 100644
--- a/usrp2/host/lib/usrp2_impl.cc
+++ b/usrp2/host/lib/usrp2_impl.cc
@@ -1293,4 +1293,161 @@ namespace usrp2 {
     return success;
   }
 
+  bool usrp2::impl::set_gpio_ddr(int bank, uint16_t value, uint16_t mask)
+  {
+    if (bank != GPIO_TX_BANK && bank != GPIO_RX_BANK) {
+      fprintf(stderr, "set_gpio_ddr: bank must be one of GPIO_RX_BANK or GPIO_TX_BANK\n");
+      return false;
+    }
+
+    op_gpio_cmd cmd;
+    op_generic_t reply;
+
+    memset(&cmd, 0, sizeof(cmd));
+    init_etf_hdrs(&cmd.h, d_addr, 0, CONTROL_CHAN, -1);
+    cmd.op.opcode = OP_GPIO_SET_DDR;
+    cmd.op.len = sizeof(cmd.op);
+    cmd.op.rid = d_next_rid++;
+    cmd.op.bank = static_cast<uint8_t>(bank);
+    cmd.op.value = htons(value);
+    cmd.op.mask = htons(mask);
+    cmd.eop.opcode = OP_EOP;
+    cmd.eop.len = sizeof(cmd.eop);
+    
+    pending_reply p(cmd.op.rid, &reply, sizeof(reply));
+    if (!transmit_cmd(&cmd, sizeof(cmd), &p, DEF_CMD_TIMEOUT))
+      return false;
+
+    bool success = (ntohx(reply.ok) == 1);
+    return success;
+  }
+
+  bool usrp2::impl::set_gpio_sels(int bank, std::string sels)
+  {
+    if (bank != GPIO_TX_BANK && bank != GPIO_RX_BANK) {
+      fprintf(stderr, "set_gpio_ddr: bank must be one of GPIO_RX_BANK or GPIO_TX_BANK\n");
+      return false;
+    }
+
+    if (sels.size() != 16) {
+      fprintf(stderr, "set_gpio_sels: sels must be exactly 16 bytes\n");
+      return false;
+    }
+
+    op_gpio_set_sels_cmd cmd;
+    op_generic_t reply;
+
+    memset(&cmd, 0, sizeof(cmd));
+    init_etf_hdrs(&cmd.h, d_addr, 0, CONTROL_CHAN, -1);
+    cmd.op.opcode = OP_GPIO_SET_SELS;
+    cmd.op.len = sizeof(cmd.op);
+    cmd.op.rid = d_next_rid++;
+    cmd.op.bank = static_cast<uint8_t>(bank);
+    memcpy(&cmd.op.sels, sels.c_str(), 16);
+    cmd.eop.opcode = OP_EOP;
+    cmd.eop.len = sizeof(cmd.eop);
+    
+    pending_reply p(cmd.op.rid, &reply, sizeof(reply));
+    if (!transmit_cmd(&cmd, sizeof(cmd), &p, DEF_CMD_TIMEOUT))
+      return false;
+
+    bool success = (ntohx(reply.ok) == 1);
+    return success;
+  }
+
+  bool usrp2::impl::write_gpio(int bank, uint16_t value, uint16_t mask)
+  {
+    if (bank != GPIO_TX_BANK && bank != GPIO_RX_BANK) {
+      fprintf(stderr, "set_gpio_ddr: bank must be one of GPIO_RX_BANK or GPIO_TX_BANK\n");
+      return false;
+    }
+
+    op_gpio_cmd cmd;
+    op_generic_t reply;
+
+    memset(&cmd, 0, sizeof(cmd));
+    init_etf_hdrs(&cmd.h, d_addr, 0, CONTROL_CHAN, -1);
+    cmd.op.opcode = OP_GPIO_WRITE;
+    cmd.op.len = sizeof(cmd.op);
+    cmd.op.rid = d_next_rid++;
+    cmd.op.bank = static_cast<uint8_t>(bank);
+    cmd.op.value = htons(value);
+    cmd.op.mask = htons(mask);
+    cmd.eop.opcode = OP_EOP;
+    cmd.eop.len = sizeof(cmd.eop);
+    
+    pending_reply p(cmd.op.rid, &reply, sizeof(reply));
+    if (!transmit_cmd(&cmd, sizeof(cmd), &p, DEF_CMD_TIMEOUT))
+      return false;
+
+    bool success = (ntohx(reply.ok) == 1);
+    return success;
+  }
+
+  bool usrp2::impl::read_gpio(int bank, uint16_t *value)
+  {
+    if (bank != GPIO_TX_BANK && bank != GPIO_RX_BANK) {
+      fprintf(stderr, "set_gpio_ddr: bank must be one of GPIO_RX_BANK or GPIO_TX_BANK\n");
+      return false;
+    }
+
+    op_gpio_cmd cmd;
+    op_gpio_read_reply_t reply;
+
+    memset(&cmd, 0, sizeof(cmd));
+    init_etf_hdrs(&cmd.h, d_addr, 0, CONTROL_CHAN, -1);
+    cmd.op.opcode = OP_GPIO_READ;
+    cmd.op.len = sizeof(cmd.op);
+    cmd.op.rid = d_next_rid++;
+    cmd.op.bank = static_cast<uint8_t>(bank);
+    cmd.op.value = 0; // not used
+    cmd.op.mask = 0;  // not used
+    cmd.eop.opcode = OP_EOP;
+    cmd.eop.len = sizeof(cmd.eop);
+    
+    pending_reply p(cmd.op.rid, &reply, sizeof(reply));
+    if (!transmit_cmd(&cmd, sizeof(cmd), &p, DEF_CMD_TIMEOUT))
+      return false;
+
+    bool success = (ntohx(reply.ok) == 1);
+    if (success && (value != NULL))
+      *value = ntohs(reply.value);
+
+    return success;
+  }
+
+  bool usrp2::impl::enable_gpio_streaming(int bank, int enable)
+  {
+    if (bank != GPIO_RX_BANK) {
+      fprintf(stderr, "enable_gpio_streaming: only RX streaming is currently implemented\n");
+      return false;
+    }
+
+    if ((enable & ~0x03) != 0) {
+      fprintf(stderr, "enable_gpio_streaming: invalid enable format\n");
+      return false;
+    }
+
+    op_gpio_cmd cmd;
+    op_generic_t reply;
+
+    memset(&cmd, 0, sizeof(cmd));
+    init_etf_hdrs(&cmd.h, d_addr, 0, CONTROL_CHAN, -1);
+    cmd.op.opcode = OP_GPIO_STREAM;
+    cmd.op.len = sizeof(cmd.op);
+    cmd.op.rid = d_next_rid++;
+    cmd.op.bank = static_cast<uint8_t>(bank);
+    cmd.op.value = htons((uint16_t)enable);
+    cmd.op.mask = 0;  // not used
+    cmd.eop.opcode = OP_EOP;
+    cmd.eop.len = sizeof(cmd.eop);
+    
+    pending_reply p(cmd.op.rid, &reply, sizeof(reply));
+    if (!transmit_cmd(&cmd, sizeof(cmd), &p, DEF_CMD_TIMEOUT))
+      return false;
+
+    bool success = (ntohx(reply.ok) == 1);
+    return success;
+  }
+
 } // namespace usrp2
diff --git a/usrp2/host/lib/usrp2_impl.h b/usrp2/host/lib/usrp2_impl.h
index c36a6d3c5..c5079a856 100644
--- a/usrp2/host/lib/usrp2_impl.h
+++ b/usrp2/host/lib/usrp2_impl.h
@@ -133,6 +133,11 @@ namespace usrp2 {
     bool set_rx_decim(int decimation_factor);
     int rx_decim() { return d_rx_decim; }
     bool set_rx_scale_iq(int scale_i, int scale_q);
+    bool set_gpio_ddr(int bank, uint16_t value, uint16_t mask);
+    bool set_gpio_sels(int bank, std::string src);
+    bool enable_gpio_streaming(int bank, int enable);
+    bool write_gpio(int bank, uint16_t value, uint16_t mask);
+    bool read_gpio(int bank, uint16_t *value);
     bool start_rx_streaming(unsigned int channel, unsigned int items_per_frame);
     bool rx_samples(unsigned int channel, rx_sample_handler *handler);
     bool stop_rx_streaming(unsigned int channel);