[Commit-gnuradio] r8541 - in usrp2/trunk/host-ng: apps include/usrp2 lib

[jcorgan]

Author: jcorgan
Date: 2008-06-01 19:02:26 -0600 (Sun, 01 Jun 2008)
New Revision: 8541

Added:
   usrp2/trunk/host-ng/lib/usrp2_thread.h
Modified:
   usrp2/trunk/host-ng/apps/test_usrp2.cc
   usrp2/trunk/host-ng/include/usrp2/usrp2.h
   usrp2/trunk/host-ng/lib/Makefile.am
   usrp2/trunk/host-ng/lib/ethernet.cc
   usrp2/trunk/host-ng/lib/ethernet.h
   usrp2/trunk/host-ng/lib/find.cc
   usrp2/trunk/host-ng/lib/pktfilter.cc
   usrp2/trunk/host-ng/lib/usrp2.cc
Log:
wip

Modified: usrp2/trunk/host-ng/apps/test_usrp2.cc
===================================================================
--- usrp2/trunk/host-ng/apps/test_usrp2.cc      2008-06-01 19:23:57 UTC (rev 
8540)
+++ usrp2/trunk/host-ng/apps/test_usrp2.cc      2008-06-02 01:02:26 UTC (rev 
8541)
@@ -21,16 +21,23 @@
 #endif
 
 #include <usrp2/usrp2.h>
-#include <iostream.h>
+#include <iostream>
 
 int
 main(int argc, char **argv)
 {
-  usrp2::props_vector_t u2s = usrp2::find("eth0");
-  std::cout << u2s.size() << std::endl;
-
-  //usrp2::usrp2::sptr u2 = usrp2::usrp2::make("eth0");
-  //std::cout << u2->foo(1) << std::endl;
+  // usrp2::usrp2::sptr u2 = usrp2::usrp2::make("eth0", "00:50:c2:85:31:67");
+  // usrp2::usrp2::sptr u2 = usrp2::usrp2::make("eth0", "BO:GU:SA:DD:RE:SS");
+  usrp2::usrp2::sptr u2 = usrp2::usrp2::make("eth0");
   
+  u2->start_rx_streaming();
+  
+  struct timespec ts;
+  ts.tv_sec = 1;
+  nanosleep(&ts, 0);
+  std::cout << std::endl;
+  
+  u2->stop_rx_streaming();
+  
   return 0;
 }

Modified: usrp2/trunk/host-ng/include/usrp2/usrp2.h
===================================================================
--- usrp2/trunk/host-ng/include/usrp2/usrp2.h   2008-06-01 19:23:57 UTC (rev 
8540)
+++ usrp2/trunk/host-ng/include/usrp2/usrp2.h   2008-06-02 01:02:26 UTC (rev 
8541)
@@ -43,13 +43,6 @@
 
 class usrp2 : boost::noncopyable
 {
-private:
-  // Only usrp2::make factory function can instantiate this class
-  usrp2(const std::string &ifc, const std::string &addr);
-  
-  class impl;
-  std::auto_ptr<impl> d_impl;
-  
 public:
   /*!
    * Shared pointer to this class
@@ -70,9 +63,41 @@
    */
   ~usrp2();  
 
-  // -------------------------------------------------------------------
+  /*!
+   * Set received sample format
+   *   
+   *    domain: complex or real
+   *      type: floating, fixed point, or raw
+   *     depth: bits per sample
+   *
+   * Sets format over the wire for samples from USRP2 and determines 
+   * default reader and writer functors   
+   * 
+   */
+  // bool set_rx_format(...);
 
-  int foo(int bar);
+  /*!
+   * Start streaming receive mode.  USRP2 will send a continuous stream of
+   * DSP pipeline samples to host.  Call rx_samples(...) to access.
+   * 
+   * \param items_per_frame  Interpreted according to current receive format
+   */
+  bool start_rx_streaming(unsigned int items_per_frame=0);
+  
+  /*!
+   * Stop streaming receive mode.
+   */
+  bool stop_rx_streaming();
+
+  /* Ignore :-) */
+  class impl;
+
+private:
+  // Only usrp2::make factory function can instantiate this class
+  usrp2(const std::string &ifc, const std::string &addr);
+  
+  // All private state is held in opaque pointer
+  std::auto_ptr<impl> d_impl;
 };
 
 };

Modified: usrp2/trunk/host-ng/lib/Makefile.am
===================================================================
--- usrp2/trunk/host-ng/lib/Makefile.am 2008-06-01 19:23:57 UTC (rev 8540)
+++ usrp2/trunk/host-ng/lib/Makefile.am 2008-06-02 01:02:26 UTC (rev 8541)
@@ -33,4 +33,6 @@
 
 # Private headers not needed for above the API development
 noinst_HEADERS = \
-       ethernet.h
+       ethernet.h \
+       pktfilter.h \
+       usrp2_thread.h

Modified: usrp2/trunk/host-ng/lib/ethernet.cc
===================================================================
--- usrp2/trunk/host-ng/lib/ethernet.cc 2008-06-01 19:23:57 UTC (rev 8540)
+++ usrp2/trunk/host-ng/lib/ethernet.cc 2008-06-02 01:02:26 UTC (rev 8541)
@@ -20,8 +20,6 @@
 #include "config.h"
 #endif
 
-#define ETHERNET_DEBUG 1
-
 #include "ethernet.h"
 #include "pktfilter.h"
 
@@ -119,18 +117,12 @@
 
 ethernet::ethernet ()
 {
-  if (ETHERNET_DEBUG)
-    std::cerr << "usrp2::ethernet: constructor" << std::endl;
-    
   d_fd = -1;
   memset (d_mac, 0, sizeof (d_mac));
 }
 
 ethernet::~ethernet ()
 {
-  if (ETHERNET_DEBUG)
-    std::cerr << "usrp2::ethernet: destructor" << std::endl;
-    
   close ();
 }
 

Modified: usrp2/trunk/host-ng/lib/ethernet.h
===================================================================
--- usrp2/trunk/host-ng/lib/ethernet.h  2008-06-01 19:23:57 UTC (rev 8540)
+++ usrp2/trunk/host-ng/lib/ethernet.h  2008-06-02 01:02:26 UTC (rev 8541)
@@ -31,8 +31,8 @@
  * via raw (non-IP) ethernet frames.
  */
 class ethernet {
-  int          d_fd;
-  unsigned char        d_mac[6];
+  int    d_fd;
+  uint8_t d_mac[6];
 
 public:
   ethernet ();
@@ -59,7 +59,7 @@
   /*!
    * \brief return 6 byte string containing our MAC address
    */
-  const unsigned char *mac () const { return d_mac; }
+  const uint8_t *mac () const { return d_mac; }
 
   /*!
    * \brief Return file descriptor associated with socket.

Modified: usrp2/trunk/host-ng/lib/find.cc
===================================================================
--- usrp2/trunk/host-ng/lib/find.cc     2008-06-01 19:23:57 UTC (rev 8540)
+++ usrp2/trunk/host-ng/lib/find.cc     2008-06-02 01:02:26 UTC (rev 8541)
@@ -29,26 +29,13 @@
 
 #define FIND_DEBUG 1
 
-std::ostream& operator<<(std::ostream &os, const u2_mac_addr_t &x)
-{
-  char buf[128];
-
-  snprintf(buf, sizeof(buf),
-          "%02x:%02x:%02x:%02x:%02x:%02x",
-          x.addr[0],x.addr[1],x.addr[2],
-          x.addr[3],x.addr[4],x.addr[5]);
-
-  os << buf;
-  return os;
-}
-
 namespace usrp2 {
 
 props_vector_t
 find(const std::string &ifc, const std::string &addr)
 {
   if (FIND_DEBUG) {
-    std::cerr << "Searching interface " << ifc << " for "
+    std::cerr << "usrp2::find: Searching interface " << ifc << " for "
               << (addr == "" ? "all USRP2s" : addr)
              << std::endl;
   }

Modified: usrp2/trunk/host-ng/lib/pktfilter.cc
===================================================================
--- usrp2/trunk/host-ng/lib/pktfilter.cc        2008-06-01 19:23:57 UTC (rev 
8540)
+++ usrp2/trunk/host-ng/lib/pktfilter.cc        2008-06-02 01:02:26 UTC (rev 
8541)
@@ -33,8 +33,6 @@
 #include <linux/types.h>
 #include <linux/filter.h>      // packet filter
 
-#define PKTFILTER_DEBUG 1
-
 /*
  * This is all based on the Berkeley Packet Filter (BPF) as implemented on 
Linux.
  *
@@ -53,14 +51,11 @@
 pktfilter::pktfilter ()
   : d_len (0), d_inst (0)
 {
-  if (PKTFILTER_DEBUG)
-    std::cerr << "pktfilter: constructor" << std::endl;
+  // NOP
 }
 
 pktfilter::~pktfilter ()
 {
-  if (PKTFILTER_DEBUG)
-    std::cerr << "pktfilter: destructor" << std::endl;
   delete [] d_inst;
 }
 

Modified: usrp2/trunk/host-ng/lib/usrp2.cc
===================================================================
--- usrp2/trunk/host-ng/lib/usrp2.cc    2008-06-01 19:23:57 UTC (rev 8540)
+++ usrp2/trunk/host-ng/lib/usrp2.cc    2008-06-02 01:02:26 UTC (rev 8541)
@@ -21,21 +21,59 @@
 #endif
 
 #include <usrp2/usrp2.h>
+#include <usrp2_eth_packet.h>
+#include "ethernet.h"
+#include "pktfilter.h"
+#include "usrp2_thread.h"
+
+#include <boost/scoped_ptr.hpp>
 #include <iostream>
+#include <stdexcept>
+#include <netinet/in.h>
+#include <errno.h>
 
+#define USRP2_DEBUG 1
+
+#if USRP2_DEBUG
+#define DEBUG_LOG(x) ::write(2, x, 1)
+#else
+#define DEBUG_LOG(x)
+#endif
+
+#define U2_ETHERTYPE 0xBEEF
+
 namespace usrp2 {
 
 // --- Private implementation class -------------------------------------
 
 class usrp2::impl
 {
+  ethernet     *d_ethernet;
+  pktfilter    *d_pf;
+  std::string   d_addr;
+  usrp2_thread *d_thread;
+  bool          d_tx_active;
+  unsigned int  d_rx_seqno;
+  unsigned int  d_tx_seqno;
+    
 public:
   impl(const std::string &ifc, const std::string &addr);
-
-  int foo(int bar);
+  ~impl();
+  
+  static bool parse_mac_addr(const std::string &s, u2_mac_addr_t *p);
+  void init_et_hdrs(u2_eth_packet_t *p, const std::string &dst);
+  void init_etf_hdrs(u2_eth_packet_t *p, const std::string &dst,
+                    int word0_flags, int chan, uint32_t timestamp);
+                    
+  bool start_rx_streaming(unsigned int items_per_frame);
+  bool stop_rx_streaming();
+  
+  void loop_body();
+  void rx_frames();
+  void tx_frames();
 };
 
-// --- Object creation glue ---------------------------------------------
+// ----------------------------------------------------------------------
 
 // Shared pointer factory function, wraps constructor call
 usrp2::sptr
@@ -57,26 +95,280 @@
   // NOP
 }
 
-// --- Proxy methods ----------------------------------------------------
+// ----------------------------------------------------------------------
 
-int
-usrp2::foo(int bar)
+bool
+usrp2::start_rx_streaming(unsigned int items_per_frame)
 {
-  return d_impl->foo(bar);
+  return d_impl->start_rx_streaming(items_per_frame);
 }
 
-// --- Private implementation -------------------------------------------
+bool
+usrp2::stop_rx_streaming()
+{
+  return d_impl->stop_rx_streaming();
+}
 
-// Impl constructor
+// ----------------------------------------------------------------------
+
 usrp2::impl::impl(const std::string &ifc, const std::string &addr)
+ : d_ethernet(new ethernet()), d_pf(0), d_thread(0),
+   d_rx_seqno(0), d_tx_seqno(0)
 {
-  std::cerr << "usrp2: constructor" << std::endl;
+  props_vector_t u2s = find(ifc, addr);
+  if (u2s.size() != 1)
+    throw std::runtime_error("Unable to find requested USRP2.");
+
+  if (!d_ethernet->open(ifc, htons(U2_ETHERTYPE)))
+    throw std::runtime_error("Unable to register USRP2 protocol");
+
+  d_pf = pktfilter::make_ethertype_inbound(U2_ETHERTYPE, d_ethernet->mac());
+  if (!d_pf || !d_ethernet->attach_pktfilter(d_pf))
+    throw std::runtime_error("Unable to attach packet filter.");
+
+  d_addr = u2s[0].addr;
+
+  if (USRP2_DEBUG)
+    std::cerr << "usrp2 constructor: using USRP2 at " << d_addr << std::endl;
+
+  d_thread = new usrp2_thread(this);
+  d_thread->start();
 }
 
-int
-usrp2::impl::foo(int bar)
+usrp2::impl::~impl()
 {
-  return bar+1;
+  d_thread->stop();
+  d_thread = 0; // thread class deletes itself
+  delete d_pf;
+  d_ethernet->close();
+  delete d_ethernet;
 }
 
+bool
+usrp2::impl::parse_mac_addr(const std::string &s, u2_mac_addr_t *p)
+{
+  p->addr[0] = 0x00;           // Matt's IAB
+  p->addr[1] = 0x50;
+  p->addr[2] = 0xC2;
+  p->addr[3] = 0x85;
+  p->addr[4] = 0x30;
+  p->addr[5] = 0x00;
+
+  int len = s.size();
+  std::cerr << len << std::endl;
+  
+  switch (len){
+
+  case 5:
+    return sscanf(s.c_str(), "%hhx:%hhx", &p->addr[4], &p->addr[5]) == 2;
+
+  case 17:
+    return sscanf(s.c_str(), "%hhx:%hhx:%hhx:%hhx:%hhx:%hhx",
+                 &p->addr[0], &p->addr[1], &p->addr[2],
+                 &p->addr[3], &p->addr[4], &p->addr[5]) == 6;
+  default:
+    return false;
+  }
 }
+
+void
+usrp2::impl::init_et_hdrs(u2_eth_packet_t *p, const std::string &dst)
+{
+  p->ehdr.ethertype = htons(U2_ETHERTYPE);
+  parse_mac_addr(dst, &p->ehdr.dst); 
+  memcpy(&p->ehdr.src, d_ethernet->mac(), 6);
+  p->thdr.flags = 0; // FIXME transport header values?
+  p->thdr.seqno = d_tx_seqno++;
+  p->thdr.ack = 0;
+}
+
+void 
+usrp2::impl::init_etf_hdrs(u2_eth_packet_t *p, const std::string &dst,
+              int word0_flags, int chan, uint32_t timestamp)
+{
+  init_et_hdrs(p, dst);
+  u2p_set_word0(&p->fixed, 0, chan);
+  u2p_set_timestamp(&p->fixed, timestamp);
+  
+  if (chan == CONTROL_CHAN) { // no sequence numbers, back it out
+    p->thdr.seqno = 0;
+    d_tx_seqno--;
+  }
+}
+
+bool
+usrp2::impl::start_rx_streaming(unsigned int items_per_frame)
+{
+  // Assume for now rx format is complex floats
+  if (items_per_frame == 0)
+    items_per_frame = 250; // TODO: calculate from d_itemsize;
+    
+  uint8_t pktbuf[ethernet::MAX_PKTLEN];
+  memset(pktbuf, 0, sizeof(pktbuf));
+  
+  struct command {
+    u2_eth_packet_t            h;
+    op_start_rx_streaming_t    op;
+    op_eop_t                   eop;
+  };
+
+  command *c = (command *) pktbuf;
+  init_etf_hdrs(&c->h, d_addr, 0, CONTROL_CHAN, -1);
+
+  c->op.opcode = OP_START_RX_STREAMING;
+  c->op.len = sizeof(op_start_rx_streaming_t);
+  c->op.items_per_frame = htonl(items_per_frame);
+
+  c->eop.opcode = OP_EOP;
+  c->eop.len = sizeof(op_eop_t);
+
+  int len = std::max((size_t) ethernet::MIN_PKTLEN, sizeof(command));
+  if (d_ethernet->write_packet(c, len) != len)
+    return false;
+
+  return true;
+}
+
+bool
+usrp2::impl::stop_rx_streaming()
+{
+}
+
+void
+usrp2::impl::loop_body()
+{
+  fd_set read_fds, write_fds;
+  FD_ZERO(&read_fds);
+  FD_ZERO(&write_fds);
+
+  FD_SET(d_ethernet->fd(), &read_fds);
+
+  if (d_tx_active)
+    FD_SET(d_ethernet->fd(), &write_fds);
+    
+  struct timeval timeout;
+  timeout.tv_sec = 0;
+  timeout.tv_usec = 100 * 1000;        // 100 ms
+
+  DEBUG_LOG("S"); // select
+  int r = select(d_ethernet->fd()+1, &read_fds, &write_fds, 0, &timeout);
+  if (r > 0) {       // Socket available for read or for write
+    if (FD_ISSET(d_ethernet->fd(), &read_fds)) {
+      DEBUG_LOG("r"); // data to read()
+      rx_frames();
+    }
+    
+    if (FD_ISSET(d_ethernet->fd(), &write_fds)) {
+      DEBUG_LOG("w"); // can write()
+      tx_frames();
+    }
+  }
+  else if (r == 0) { 
+    DEBUG_LOG("T");   // socket timeout
+    return;
+  }
+  else {         
+    if (errno == EINTR)
+      DEBUG_LOG("I"); // interrupted system call
+    else
+      DEBUG_LOG("!"); // error on socket
+    return;
+  }
+}
+
+void
+usrp2::impl::rx_frames()
+{
+  uint8_t pktbuf[ethernet::MAX_PKTLEN];
+  memset(pktbuf, 0, sizeof(pktbuf));
+  
+  int len;
+  while (1) {
+    len = d_ethernet->read_packet_dont_block(pktbuf, sizeof(pktbuf));
+    
+    if (len < 0) {
+      DEBUG_LOG("!"); // error
+      return;
+    }
+    else if (len == 0) {
+      DEBUG_LOG("|"); // no more frames this select
+      return;
+    }
+    else if ((size_t)len < sizeof(u2_eth_packet_t)) {
+      DEBUG_LOG("<"); // short packet
+    }
+    else {
+      DEBUG_LOG("R"); // read ok
+    }
+
+   // Do stuff with received packet...
+    
+  }
+}
+
+void
+usrp2::impl::tx_frames()
+{
+}
+
+// ----------------------------------------------------------------------
+
+usrp2_thread::usrp2_thread(usrp2::impl *u2) :
+  omni_thread(NULL, PRIORITY_HIGH),
+  d_u2(u2), d_keep_running(false)
+{
+}
+
+usrp2_thread::~usrp2_thread()
+{
+  if (d_keep_running)
+    stop();
+
+  // we don't own this, just forget it
+  d_u2 = 0;
+}
+
+void
+usrp2_thread::start()
+{
+  if (USRP2_DEBUG)
+    std::cerr << "usrp2_thread::start() "
+             << this << std::endl;
+  d_keep_running = true;
+  start_undetached();
+}
+
+void *
+usrp2_thread::run_undetached(void *arg)
+{
+#if 0
+  // FIXME hoist this from gnuradio-core into another library.
+  gr_rt_status_t rt = gr_enable_realtime_scheduling();
+  if (rt != RT_OK)
+    std::cerr << "failed to enable realtime scheduling\n";
+#endif
+
+  // This is the first code to run in the new thread context.
+  while(d_keep_running)
+    d_u2->loop_body();
+
+  return 0;
+}
+
+void
+usrp2_thread::stop()
+{
+  if (USRP2_DEBUG)
+    std::cerr << "usrp2_thread::stop: joining background thread "
+             << this << std::endl;
+  d_keep_running = false;
+
+  void *dummy_status;
+  join(&dummy_status);  
+
+  if (USRP2_DEBUG)
+    std::cerr << "usrp2_thread::stop: join returned" << std::endl;
+}
+
+} // namespace usrp2
+

Added: usrp2/trunk/host-ng/lib/usrp2_thread.h
===================================================================
--- usrp2/trunk/host-ng/lib/usrp2_thread.h                              (rev 0)
+++ usrp2/trunk/host-ng/lib/usrp2_thread.h      2008-06-02 01:02:26 UTC (rev 
8541)
@@ -0,0 +1,50 @@
+/* -*- c++ -*- */
+/*
+ * Copyright 2008 Free Software Foundation, Inc.
+ * 
+ * This file is part of GNU Radio
+ * 
+ * GNU Radio 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, or (at your option)
+ * any later version.
+ * 
+ * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
+ * the Free Software Foundation, Inc., 51 Franklin Street,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifndef INCLUDED_USRP2_THREAD_H
+#define INCLUDED_USRP2_THREAD_H
+
+#include <omnithread.h>
+
+namespace usrp2 {
+
+class usrp2::impl;
+
+class usrp2_thread : public omni_thread
+{
+private:
+  usrp2::impl *d_u2;    
+  bool         d_keep_running;
+
+public:
+  usrp2_thread(usrp2::impl *u2);
+  ~usrp2_thread();
+
+  void start();
+  void stop();
+
+  virtual void *run_undetached(void *arg);
+};
+
+} // namespace usrp2
+
+#endif /* INCLUDED_USRP2_THREAD_H */