git.gag.com Git - debian/gnuradio/blob - usrp/limbo/inband/usrp_server.cc

   1 /* -*- c++ -*- */
   2 /*
   3  * Copyright 2007,2008 Free Software Foundation, Inc.
   4  *
   5  * This file is part of GNU Radio
   6  *
   7  * GNU Radio is free software; you can redistribute it and/or modify
   8  * it under the terms of the GNU General Public License as published by
   9  * the Free Software Foundation; either version 3, or (at your option)
  10  * any later version.
  11  *
  12  * GNU Radio is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  15  * GNU General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU General Public License along
  18  * with this program; if not, write to the Free Software Foundation, Inc.,
  19  * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
  20  */
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25 #include <usrp_server.h>
  26 #include <iostream>
  27 #include <usrp_inband_usb_packet.h>
  28 #include <mblock/class_registry.h>
  29 #include <vector>
  30 #include <usrp_usb_interface.h>
  31 #include <string.h>
  32 #include <fpga_regs_common.h>
  33 #include <fpga_regs_standard.h>
  34
  35 #include <symbols_usrp_server_cs.h>
  36 #include <symbols_usrp_channel.h>
  37 #include <symbols_usrp_tx.h>
  38 #include <symbols_usrp_rx.h>
  39 #include <symbols_usrp_low_level_cs.h>
  40 #include <symbols_usrp_interface_cs.h>
  41
  42 static pmt_t s_shutdown = pmt_intern("%shutdown");
  43
  44 typedef usrp_inband_usb_packet transport_pkt;   // makes conversion to gigabit easy
  45
  46 const static bool verbose = false;
  47
  48 static std::string
  49 str(long x)
  50 {
  51   std::ostringstream s;
  52   s << x;
  53   return s.str();
  54 }
  55
  56 usrp_server::usrp_server(mb_runtime *rt, const std::string &instance_name, pmt_t user_arg)
  57   : mb_mblock(rt, instance_name, user_arg),
  58   d_fpga_debug(false),
  59   d_interp_tx(128),     // these should match the lower level defaults (rx also)
  60   d_decim_rx(128),
  61   d_fake_rx(false)
  62 {
  63   if(verbose)
  64     std::cout << "[USRP_SERVER] Initializing...\n";
  65
  66   // Dictionary for arguments to all of the components
  67   d_usrp_dict = user_arg;
  68
  69   if (pmt_is_dict(d_usrp_dict)) {
  70
  71     if(pmt_t fpga_debug = pmt_dict_ref(d_usrp_dict,
  72                                       pmt_intern("fpga-debug"),
  73                                       PMT_NIL)) {
  74       if(pmt_eqv(fpga_debug, PMT_T))
  75         d_fpga_debug=true;
  76     }
  77
  78     // Read the TX interpolations
  79     if(pmt_t interp_tx = pmt_dict_ref(d_usrp_dict,
  80                                       pmt_intern("interp-tx"),
  81                                       PMT_NIL)) {
  82       if(!pmt_eqv(interp_tx, PMT_NIL))
  83         d_interp_tx = pmt_to_long(interp_tx);
  84     }
  85
  86     // Read the RX decimation rate
  87     if(pmt_t decim_rx = pmt_dict_ref(d_usrp_dict,
  88                                       pmt_intern("decim-rx"),
  89                                       PMT_NIL)) {
  90       if(!pmt_eqv(decim_rx, PMT_NIL))
  91         d_decim_rx = pmt_to_long(decim_rx);
  92     }
  93   }
  94
  95   // control & status port
  96   d_cs = define_port("cs", "usrp-server-cs", true, mb_port::EXTERNAL);
  97   d_cs_usrp = define_port("cs_usrp", "usrp-interface-cs", false, mb_port::INTERNAL);
  98
  99   // ports
 100   //
 101   // (if/when we do replicated ports, these will be replaced by a
 102   //  single replicated port)
 103   for(int port=0; port < N_PORTS; port++) {
 104
 105     d_tx.push_back(define_port("tx"+str(port),
 106                                "usrp-tx",
 107                                true,
 108                                mb_port::EXTERNAL));
 109
 110     d_rx.push_back(define_port("rx"+str(port),
 111                                "usrp-rx",
 112                                true,
 113                                mb_port::EXTERNAL));
 114   }
 115
 116   define_component("usrp", "usrp_usb_interface", d_usrp_dict);
 117   connect("self", "cs_usrp", "usrp", "cs");
 118
 119   d_defer=false;
 120   d_opened=false;
 121
 122   // FIXME: needs to be returned from open, if we want to use this
 123   d_nrx_chan = 2;
 124   d_ntx_chan = 2;
 125
 126   // Initialize capacity on each channel to 0 and to no owner
 127   // Also initialize the USRP standard tx/rx pointers to NULL
 128   for(int chan=0; chan < d_ntx_chan; chan++)
 129     d_chaninfo_tx.push_back(channel_info());
 130
 131   for(int chan=0; chan < d_nrx_chan; chan++)
 132     d_chaninfo_rx.push_back(channel_info());
 133
 134   d_rx_chan_mask = 0;
 135
 136   for(int i=0; i < D_MAX_RID; i++)
 137     d_rids.push_back(rid_info());
 138
 139   //d_fake_rx=true;
 140 }
 141
 142 /*!
 143  * \brief resets the assigned capacity and owners of each RX and TX channel from
 144  * allocations.
 145  */
 146 void
 147 usrp_server::reset_channels()
 148 {
 149
 150   for(int chan=0; chan < d_ntx_chan; chan++) {
 151     d_chaninfo_tx[chan].assigned_capacity = 0;
 152     d_chaninfo_tx[chan].owner = PMT_NIL;
 153   }
 154
 155   for(int chan=0; chan < d_nrx_chan; chan++) {
 156     d_chaninfo_rx[chan].assigned_capacity = 0;
 157     d_chaninfo_rx[chan].owner = PMT_NIL;
 158   }
 159
 160   d_rx_chan_mask = 0;
 161 }
 162
 163 usrp_server::~usrp_server()
 164 {
 165 }
 166
 167
 168 void
 169 usrp_server::initial_transition()
 170 {
 171   // the initial transition
 172 }
 173
 174 /*!
 175  * \brief Reads all incoming messages to USRP server from the TX, RX, and the CS
 176  * ports.  This drives the state of USRP server and dispatches based on the
 177  * message.
 178  */
 179 void
 180 usrp_server::handle_message(mb_message_sptr msg)
 181 {
 182   pmt_t event = msg->signal();          // the "name" of the message
 183   pmt_t port_id = msg->port_id();       // which port it came in on
 184   pmt_t data = msg->data();
 185   pmt_t invocation_handle;
 186   pmt_t metadata = msg->metadata();
 187   pmt_t status;
 188
 189   long port;
 190
 191   if (pmt_eq(event, s_shutdown))        // ignore (for now)
 192     return;
 193
 194   invocation_handle = pmt_nth(0, data);
 195
 196   if (0){
 197     std::cout << "[USRP_SERVER] event: " << event << std::endl;
 198     std::cout << "[USRP_SERVER] port_id: " << port_id << std::endl;
 199   }
 200
 201   // It would be nice if this were all table driven, and we could compute our
 202   // state transition as f(current_state, port_id, signal)
 203
 204   // A message from the USRP CS, which should *only* be responses
 205   //
 206   // It is important that this set come before checking messages of any other
 207   // components.  This is since we always want to listen to the low level USRP
 208   // server, even if we aren't initialized we are waiting for responses to
 209   // become initialized.  Likewise, after the usrp_server is "closed", we still
 210   // want to pass responses back from the low level.
 211
 212   //---------------- USRP RESPONSE ---------------//
 213   if (pmt_eq(port_id, d_cs_usrp->port_symbol())) {
 214
 215     //-------------- USRP OPEN ------------------//
 216     if(pmt_eq(event, s_response_usrp_open)) {
 217       // pass the response back over the regular CS port
 218       pmt_t status = pmt_nth(1, data);
 219       d_cs->send(s_response_open, pmt_list2(invocation_handle, status));
 220
 221       //reset_all_registers();
 222       //initialize_registers();
 223
 224       if(pmt_eqv(status,PMT_T)) {
 225         d_opened = true;
 226         d_defer = false;
 227         recall_defer_queue();
 228       }
 229
 230       return;
 231     }
 232     //------------- USRP CLOSE -------------------//
 233     else if (pmt_eq(event, s_response_usrp_close)) {
 234       pmt_t status = pmt_nth(1, data);
 235       d_cs->send(s_response_close, pmt_list2(invocation_handle, status));
 236
 237       if(pmt_eqv(status,PMT_T)) {
 238         d_opened = false;
 239         d_defer = false;
 240         reset_channels();
 241         recall_defer_queue();
 242       }
 243
 244       return;
 245     }
 246     //--------------- USRP WRITE --------------//
 247     else if (pmt_eq(event, s_response_usrp_write)) {
 248
 249       pmt_t status = pmt_nth(1, data);
 250       long channel = pmt_to_long(pmt_nth(2, data));
 251       long port;
 252
 253       // Do not report back responses if they were generated from a
 254       // command packet
 255       if(channel == CONTROL_CHAN)
 256         return;
 257
 258       // Find the port through the owner of the channel
 259       if((port = tx_port_index(d_chaninfo_tx[channel].owner)) !=-1 )
 260         d_tx[port]->send(s_response_xmit_raw_frame,
 261                          pmt_list2(invocation_handle, status));
 262       return;
 263     }
 264     //--------------- USRP READ ---------------//
 265     else if (pmt_eq(event, s_response_usrp_read)) {
 266
 267       pmt_t status = pmt_nth(1, data);
 268
 269       if(!pmt_eqv(status, PMT_T)) {
 270         std::cerr << "[USRP_SERVER] Error receiving packet\n";
 271         return;
 272       }
 273       else {
 274         handle_response_usrp_read(data);
 275         return;
 276       }
 277     }
 278
 279     goto unhandled;
 280   }
 281
 282   // Checking for defer on all other messages
 283   if(d_defer) {
 284     if (verbose)
 285       std::cout << "[USRP_SERVER] Received msg while deferring ("
 286                 << msg->signal() << ")\n";
 287     d_defer_queue.push(msg);
 288     return;
 289   }
 290
 291   //--------- CONTROL / STATUS ------------//
 292   if (pmt_eq(port_id, d_cs->port_symbol())){
 293
 294     //----------- OPEN -----------//
 295     if (pmt_eq(event, s_cmd_open)){
 296
 297       // Reject if already open
 298       if(d_opened) {
 299         d_cs->send(s_response_open, pmt_list2(invocation_handle, s_err_usrp_already_opened));
 300         return;
 301       }
 302
 303       // the parameters are the same to the low level interface, so we just pass 'data' along
 304       d_cs_usrp->send(s_cmd_usrp_open, data);
 305
 306       d_defer = true;
 307
 308       return;
 309     }
 310     //---------- CLOSE -----------//
 311     else if (pmt_eq(event, s_cmd_close)){
 312
 313       if(!d_opened) {
 314         d_cs->send(s_response_close, pmt_list2(invocation_handle, s_err_usrp_already_closed));
 315         return;
 316       }
 317
 318       d_defer = true;
 319       d_cs_usrp->send(s_cmd_usrp_close, pmt_list1(invocation_handle));
 320
 321       return;
 322     }
 323     //---------- MAX CAPACITY ----------//
 324     else if (pmt_eq(event, s_cmd_max_capacity)) {
 325
 326       if(!d_opened) {
 327         d_cs->send(s_response_max_capacity,
 328                    pmt_list3(invocation_handle, s_err_usrp_not_opened, pmt_from_long(0)));
 329         return;
 330       }
 331
 332       d_cs->send(s_response_max_capacity,
 333                  pmt_list3(invocation_handle,
 334                            PMT_T,
 335                            pmt_from_long(max_capacity())));
 336       return;
 337     }
 338     //---------- NTX CHAN --------------//
 339     else if (pmt_eq(event, s_cmd_ntx_chan)) {
 340
 341       if(!d_opened) {
 342         d_cs->send(s_response_ntx_chan,
 343                    pmt_list3(invocation_handle, s_err_usrp_not_opened, pmt_from_long(0)));
 344         return;
 345       }
 346
 347       d_cs->send(s_response_ntx_chan,
 348                  pmt_list3(invocation_handle,
 349                            PMT_T,
 350                            pmt_from_long(d_ntx_chan)));
 351       return;
 352     }
 353     //---------- NRX CHAN -----------//
 354     else if (pmt_eq(event, s_cmd_nrx_chan)) {
 355
 356       if(!d_opened) {
 357         d_cs->send(s_response_nrx_chan,
 358                    pmt_list3(invocation_handle, s_err_usrp_not_opened, pmt_from_long(0)));
 359         return;
 360       }
 361
 362       d_cs->send(s_response_nrx_chan,
 363                  pmt_list3(invocation_handle,
 364                            PMT_T,
 365                            pmt_from_long(d_nrx_chan)));
 366       return;
 367     }
 368     //--------- ALLOCATION? -----------//
 369     else if (pmt_eq(event, s_cmd_current_capacity_allocation)) {
 370
 371       if(!d_opened) {
 372         d_cs->send(s_response_current_capacity_allocation,
 373                    pmt_list3(invocation_handle,
 374                              s_err_usrp_not_opened,
 375                              pmt_from_long(0)));
 376         return;
 377       }
 378
 379       d_cs->send(s_response_current_capacity_allocation,
 380                  pmt_list3(invocation_handle,
 381                            PMT_T,
 382                            pmt_from_long(current_capacity_allocation())));
 383       return;
 384     }
 385     goto unhandled;
 386   }
 387
 388   //-------------- TX ---------------//
 389   if ((port = tx_port_index(port_id)) != -1) {
 390
 391     //------------ ALLOCATE (TX) ----------------//
 392     if (pmt_eq(event, s_cmd_allocate_channel)){
 393
 394       if(!d_opened) {
 395         d_tx[port]->send(s_response_allocate_channel,
 396                           pmt_list3(invocation_handle,
 397                                     s_err_usrp_not_opened,
 398                                     pmt_from_long(0)));
 399         return;
 400       }
 401
 402       handle_cmd_allocate_channel(d_tx[port], d_chaninfo_tx, data);
 403       return;
 404     }
 405
 406     //----------- DEALLOCATE (TX) ---------------//
 407     if (pmt_eq(event, s_cmd_deallocate_channel)) {
 408
 409       if(!d_opened) {
 410         d_tx[port]->send(s_response_deallocate_channel,
 411                          pmt_list3(invocation_handle,
 412                                    s_err_usrp_not_opened,
 413                                    pmt_from_long(0)));
 414         return;
 415       }
 416
 417       handle_cmd_deallocate_channel(d_tx[port], d_chaninfo_tx, data);
 418       return;
 419     }
 420
 421     //-------------- XMIT RAW FRAME -----------------/
 422     if (pmt_eq(event, s_cmd_xmit_raw_frame)){
 423
 424       if(!d_opened) {
 425         d_tx[port]->send(s_response_xmit_raw_frame,
 426                          pmt_list2(invocation_handle, s_err_usrp_not_opened));
 427         return;
 428       }
 429
 430       handle_cmd_xmit_raw_frame(d_tx[port], d_chaninfo_tx, data);
 431       return;
 432     }
 433
 434     //-------------- CONTROL PACKET -----------------/
 435     if (pmt_eq(event, s_cmd_to_control_channel)) {
 436
 437       if(!d_opened) {
 438         d_tx[port]->send(s_response_xmit_raw_frame,
 439                          pmt_list2(invocation_handle, s_err_usrp_not_opened));
 440         return;
 441       }
 442
 443       handle_cmd_to_control_channel(d_tx[port], d_chaninfo_tx, data);
 444       return;
 445
 446     }
 447
 448     goto unhandled;
 449   }
 450
 451   //-------------- RX ---------------//
 452   if ((port = rx_port_index(port_id)) != -1) {
 453
 454     //------------ ALLOCATE (RX) ----------------//
 455     if (pmt_eq(event, s_cmd_allocate_channel)) {
 456
 457       if(!d_opened) {
 458         d_rx[port]->send(s_response_allocate_channel,
 459                           pmt_list3(invocation_handle,
 460                                     s_err_usrp_not_opened,
 461                                     pmt_from_long(0)));
 462         return;
 463       }
 464
 465       handle_cmd_allocate_channel(d_rx[port], d_chaninfo_rx, data);
 466       return;
 467     }
 468
 469     //----------- DEALLOCATE (RX) ---------------//
 470     if (pmt_eq(event, s_cmd_deallocate_channel)) {
 471
 472       if(!d_opened) {
 473         d_rx[port]->send(s_response_deallocate_channel,
 474                          pmt_list3(invocation_handle,
 475                                    s_err_usrp_not_opened,
 476                                    pmt_from_long(0)));
 477         return;
 478       }
 479
 480       handle_cmd_deallocate_channel(d_rx[port], d_chaninfo_rx, data);
 481       return;
 482     }
 483
 484     //-------------- START RECV ----------------//
 485     if (pmt_eq(event, s_cmd_start_recv_raw_samples)) {
 486
 487       if(!d_opened) {
 488         d_rx[port]->send(s_response_recv_raw_samples,
 489                          pmt_list2(invocation_handle, s_err_usrp_not_opened));
 490         return;
 491       }
 492
 493       handle_cmd_start_recv_raw_samples(d_rx[port], d_chaninfo_rx, data);
 494       return;
 495     }
 496
 497     //-------------- STOP RECV ----------------//
 498     if (pmt_eq(event, s_cmd_stop_recv_raw_samples)) {
 499
 500       if(!d_opened)
 501         return;
 502
 503       // FIX ME : no response for stopping? even if error? (permissions)
 504       handle_cmd_stop_recv_raw_samples(d_rx[port], d_chaninfo_rx, data);
 505
 506       return;
 507     }
 508
 509     goto unhandled;
 510   }
 511
 512  unhandled:
 513   std::cout << "[USRP_SERVER] unhandled msg: " << msg << std::endl;
 514 }
 515
 516 /*!
 517  * \brief Takes a port_symbol() as parameter \p port_id and is used to determine
 518  * if the port is a TX port, or to find an index in the d_tx vector which stores
 519  * the port.
 520  *
 521  * \returns -1 if \p port_id is not in the d_tx vector (i.e., it's not a TX
 522  * port), otherwise returns an index in the d_tx vector which stores the port.
 523  */
 524 int usrp_server::tx_port_index(pmt_t port_id) {
 525
 526   for(int i=0; i < (int) d_tx.size(); i++)
 527     if(pmt_eq(d_tx[i]->port_symbol(), port_id))
 528       return i;
 529
 530   return -1;
 531 }
 532
 533 /*!
 534  * \brief Takes a port_symbol() as parameter \p port_id and is used to determine
 535  * if the port is an RX port, or to find an index in the d_rx vector which
 536  * stores the port.
 537  *
 538  * \returns -1 if \p port_id is not in the d_rx vector (i.e., it's not an RX
 539  * port), otherwise returns an index in the d_rx vector which stores the port.
 540  */
 541 int usrp_server::rx_port_index(pmt_t port_id) {
 542
 543   for(int i=0; i < (int) d_rx.size(); i++)
 544     if(pmt_eq(d_rx[i]->port_symbol(), port_id))
 545       return i;
 546
 547   return -1;
 548 }
 549
 550 /*!
 551  * \brief Determines the current total capacity allocated by all RX and TX
 552  * channels.
 553  *
 554  * \returns the total allocated capacity
 555  */
 556 long usrp_server::current_capacity_allocation() {
 557   long capacity = 0;
 558
 559   for(int chan=0; chan < d_ntx_chan; chan++)
 560     capacity += d_chaninfo_tx[chan].assigned_capacity;
 561
 562   for(int chan=0; chan < d_nrx_chan; chan++)
 563     capacity += d_chaninfo_rx[chan].assigned_capacity;
 564
 565   return capacity;
 566 }
 567
 568
 569 /*!
 570  * \brief Called by the handle_message() method if the incoming message to
 571  * usrp_server is to allocate a channel (cmd-allocate-channel).  The method
 572  * checks if the requested capacity exists and if so it will reserve it for the
 573  * caller on the channel that is returned via a response-allocate-channel
 574  * signal.
 575  */
 576 void
 577 usrp_server::handle_cmd_allocate_channel(
 578                                 mb_port_sptr port,
 579                                 std::vector<struct channel_info> &chan_info,
 580                                 pmt_t data)
 581 {
 582   pmt_t invocation_handle = pmt_nth(0, data);
 583   long rqstd_capacity = pmt_to_long(pmt_nth(1, data));
 584   long chan;
 585
 586   // Check capacity exists
 587   if((D_USB_CAPACITY - current_capacity_allocation()) < rqstd_capacity) {
 588
 589     // no capacity available
 590     port->send(s_response_allocate_channel,
 591                pmt_list3(invocation_handle,
 592                          s_err_requested_capacity_unavailable,
 593                          PMT_NIL));
 594     return;
 595   }
 596
 597   // Find a free channel, assign the capacity and respond
 598   for(chan=0; chan < (long)chan_info.size(); chan++) {
 599
 600     if(verbose)
 601       std::cout << "[USRP_SERVER] Checking chan: " << chan
 602                 << " owner " << chan_info[chan].owner
 603                 << " size " << chan_info.size()
 604                 << std::endl;
 605
 606     if(chan_info[chan].owner == PMT_NIL) {
 607
 608       chan_info[chan].owner = port->port_symbol();
 609       chan_info[chan].assigned_capacity = rqstd_capacity;
 610
 611       port->send(s_response_allocate_channel,
 612                  pmt_list3(invocation_handle,
 613                            PMT_T,
 614                            pmt_from_long(chan)));
 615
 616       if(verbose)
 617         std::cout << "[USRP_SERVER] Assigning channel: " << chan
 618                   << " to " << chan_info[chan].owner
 619                   << std::endl;
 620       return;
 621     }
 622
 623   }
 624
 625   if (verbose)
 626     std::cout << "[USRP_SERVER] Couldnt find a TX chan\n";
 627
 628   // no free TX chan found
 629   port->send(s_response_allocate_channel,
 630              pmt_list3(invocation_handle,
 631                        s_err_channel_unavailable,
 632                        PMT_NIL));
 633   return;
 634 }
 635
 636 /*!
 637  * \brief Called by the handle_message() method if the incoming message to
 638  * usrp_server is to deallocate a channel (cmd-deallocate-channel).  The method
 639  * ensures that the sender of the signal owns the channel and that the channel
 640  * number is valid.  A response-deallocate-channel signal is sent back with the
 641  * result of the deallocation.
 642  */
 643 void
 644 usrp_server::handle_cmd_deallocate_channel(
 645                               mb_port_sptr port,
 646                               std::vector<struct channel_info> &chan_info,
 647                               pmt_t data)
 648 {
 649
 650   pmt_t invocation_handle = pmt_nth(0, data);
 651   long channel = pmt_to_long(pmt_nth(1, data));
 652
 653   // Ensure the channel is valid and the caller owns the port
 654   if(!check_valid(port, channel, chan_info,
 655                   pmt_list2(s_response_deallocate_channel, invocation_handle)))
 656     return;
 657
 658   chan_info[channel].assigned_capacity = 0;
 659   chan_info[channel].owner = PMT_NIL;
 660
 661   port->send(s_response_deallocate_channel,
 662              pmt_list2(invocation_handle,
 663                        PMT_T));
 664   return;
 665 }
 666
 667 /*!
 668  * \brief Called by the handle_message() method if the incoming message to
 669  * usrp_server is to transmit a frame (cmd-xmit-raw-frame).  The method
 670  * allocates enough memory to support a burst of packets which contain the frame
 671  * over the bus of the frame, sets the packet headers, and sends a signal to the
 672  * lower block for the data (packets) to be written to the bus.
 673  *
 674  * The \p port the command was sent on and the channel info (\p chan_info) of
 675  * the channel the frame is to be transmitted on are passed to ensure that the
 676  * caller owns the channel.
 677  *
 678  * The \p data parameter is in the format of a cmd-xmit-raw-frame signal.
 679  *
 680  * The properties
 681  */
 682 void usrp_server::handle_cmd_xmit_raw_frame(
 683                               mb_port_sptr port,
 684                               std::vector<struct channel_info> &chan_info,
 685                               pmt_t data)
 686 {
 687   size_t n_bytes, psize;
 688   long max_payload_len = transport_pkt::max_payload();
 689
 690   pmt_t invocation_handle = pmt_nth(0, data);
 691   long channel = pmt_to_long(pmt_nth(1, data));
 692   const void *samples = pmt_uniform_vector_elements(pmt_nth(2, data), n_bytes);
 693   long timestamp = pmt_to_long(pmt_nth(3, data));
 694   pmt_t properties = pmt_nth(4, data);
 695
 696   // Ensure the channel is valid and the caller owns the port
 697   if(!check_valid(port, channel, chan_info,
 698                   pmt_list2(s_response_xmit_raw_frame, invocation_handle)))
 699     return;
 700
 701   // Read information from the properties of the packet
 702   bool carrier_sense = false;
 703   if(pmt_is_dict(properties)) {
 704
 705     // Check if carrier sense is enabled for the frame
 706     if(pmt_t p_carrier_sense = pmt_dict_ref(properties,
 707                                             pmt_intern("carrier-sense"),
 708                                             PMT_NIL)) {
 709       if(pmt_eqv(p_carrier_sense, PMT_T))
 710         carrier_sense = true;
 711     }
 712   }
 713
 714
 715   // Determine the number of packets to allocate contiguous memory for
 716   // bursting over the USB and get a pointer to the memory to be used in
 717   // building the packets
 718   long n_packets =
 719     static_cast<long>(std::ceil(n_bytes / (double)max_payload_len));
 720
 721   pmt_t v_packets = pmt_make_u8vector(sizeof(transport_pkt) * n_packets, 0);
 722
 723   transport_pkt *pkts =
 724     (transport_pkt *) pmt_u8vector_writable_elements(v_packets, psize);
 725
 726   for(int n=0; n < n_packets; n++) {
 727
 728     long payload_len =
 729       std::min((long)(n_bytes-(n*max_payload_len)), (long)max_payload_len);
 730
 731     if(n == 0) { // first packet gets start of burst flag and timestamp
 732
 733       if(carrier_sense)
 734         pkts[n].set_header(pkts[n].FL_START_OF_BURST
 735                            | pkts[n].FL_CARRIER_SENSE,
 736                            channel, 0, payload_len);
 737       else
 738         pkts[n].set_header(pkts[n].FL_START_OF_BURST, channel, 0, payload_len);
 739
 740       pkts[n].set_timestamp(timestamp);
 741
 742     } else {
 743       pkts[n].set_header(0, channel, 0, payload_len);
 744       pkts[n].set_timestamp(0xffffffff);
 745     }
 746
 747     memcpy(pkts[n].payload(),
 748            (uint8_t *)samples+(max_payload_len * n),
 749            payload_len);
 750
 751   }
 752
 753
 754   pkts[n_packets-1].set_end_of_burst(); // set the last packet's end of burst
 755
 756   if (verbose && 0)
 757     std::cout << "[USRP_SERVER] Received raw frame invocation: "
 758               << invocation_handle << std::endl;
 759
 760   // The actual response to the write will be generated by a
 761   // s_response_usrp_write since we cannot determine whether to transmit was
 762   // successful until we hear from the lower layers.
 763   d_cs_usrp->send(s_cmd_usrp_write,
 764                   pmt_list3(invocation_handle,
 765                             pmt_from_long(channel),
 766                             v_packets));
 767
 768   return;
 769 }
 770
 771 /*!
 772  * \brief Called by the handle_message() method to parse incoming control/status
 773  * signals (cmd-to-control-channel).
 774  *
 775  * The \p port the command was sent on and the channel info (\p chan_info) of
 776  * the channel are passed to ensure that the caller owns the channel.
 777  *
 778  * The \p data parameter is in the format of a PMT list, where each element
 779  * follows the format of a control/status signal (i.e. op-ping-fixed).
 780  *
 781  * The method will parse all of the C/S commands included in \p data and place
 782  * the commands in to a lower level packet sent to the control channel.  The
 783  * method will pack as many commands as possible in t oa single packet, and once
 784  * it is fill generate as many lower level packets as needed.
 785  *
 786  * Anything that needs to be returned to the sender of the signal (i.e. the
 787  * value of a register) will be generated by the parse_control_pkt() method as
 788  * the responses to the commands are read back from the USRP.
 789  */
 790 void usrp_server::handle_cmd_to_control_channel(
 791                             mb_port_sptr port,
 792                             std::vector<struct channel_info> &chan_info,
 793                             pmt_t data)
 794 {
 795
 796   pmt_t invocation_handle = pmt_nth(0, data);
 797   pmt_t subpackets = pmt_nth(1, data);
 798
 799   long n_subpkts = pmt_length(subpackets);
 800   long curr_subpkt = 0;
 801
 802   size_t psize;
 803   long payload_len = 0;
 804   long channel = CONTROL_CHAN;
 805
 806   if(verbose)
 807     std::cout << "[USRP_SERVER] Handling " << n_subpkts << " commands\n";
 808
 809   // The design of the following code is optimized for simplicity, not
 810   // performance.  To performance optimize this code, the total size in bytes
 811   // needed for all of the CS packets is needed to allocate contiguous memory
 812   // which contains the USB packets for bursting over the bus.  However to do
 813   // this the packets subpackets would need to be parsed twice and their sizes
 814   // would need to be determined.
 815   //
 816   // The approach taken is to keep parsing the subpackets and putting them in to
 817   // USB packets.  Once the USB packet is full, a write is sent for it and
 818   // another packet is created.
 819   //
 820   // The subpacket creation methods will return false if the subpacket will not
 821   // fit in to the current USB packet.  In these cases a new USB packet is
 822   // created and the old is sent.
 823
 824   new_packet:
 825     // This code needs to become "smart" and only make a new packet when full
 826     pmt_t v_packet = pmt_make_u8vector(sizeof(transport_pkt), 0);
 827     transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_packet, psize);
 828     payload_len = 0;
 829
 830     pkt->set_header(0, channel, 0, payload_len);
 831     pkt->set_timestamp(0xffffffff);
 832
 833   while(curr_subpkt < n_subpkts) {
 834
 835     pmt_t subp = pmt_nth(curr_subpkt, subpackets);
 836     pmt_t subp_cmd = pmt_nth(0, subp);
 837     pmt_t subp_data = pmt_nth(1, subp);
 838
 839     //--------- PING FIXED --------------//
 840     if(pmt_eq(subp_cmd, s_op_ping_fixed)) {
 841
 842       long urid     = pmt_to_long(pmt_nth(0, subp_data));
 843       long pingval  = pmt_to_long(pmt_nth(1, subp_data));
 844
 845       // USRP server sets request ID's to keep track of which application gets
 846       // what response back.  To allow a full 6-bits for an RID to the user, we
 847       // keep a mapping and replace the RID's as the packets go in and out.  If
 848       // there are no RID's available, the command is thrown away silently.
 849       long srid;
 850       if((srid = next_rid()) == -1)
 851         goto subpkt_bail;
 852
 853       // We use a vector to store the owner of the ping request and will use it
 854       // to send the request on any RX port they own.
 855       d_rids[srid].owner = port->port_symbol();
 856       d_rids[srid].user_rid = urid;
 857
 858       // Adds a ping after the previous command in the pkt
 859       if(!pkt->cs_ping(srid, pingval))
 860       {
 861         d_cs_usrp->send(s_cmd_usrp_write,
 862                         pmt_list3(invocation_handle,
 863                                   pmt_from_long(channel),
 864                                   v_packet));
 865
 866         // Return the RID
 867         d_rids[srid].owner = PMT_NIL;
 868
 869         goto new_packet;
 870       }
 871
 872       if(verbose)
 873         std::cout << "[USRP_SERVER] Received ping command request"
 874                   << " assigning RID " << srid << std::endl;
 875
 876     }
 877
 878     //----------- WRITE REG ---------------//
 879     if(pmt_eq(subp_cmd, s_op_write_reg)) {
 880
 881       long reg_num = pmt_to_long(pmt_nth(0, subp_data));
 882       long val = pmt_to_long(pmt_nth(1, subp_data));
 883
 884       if(!pkt->cs_write_reg(reg_num, val))
 885       {
 886         d_cs_usrp->send(s_cmd_usrp_write,
 887                         pmt_list3(invocation_handle,
 888                                   pmt_from_long(channel),
 889                                   v_packet));
 890
 891         goto new_packet;
 892       }
 893
 894       if(verbose)
 895         std::cout << "[USRP_SERVER] Received write register request "
 896                   << "("
 897                   << "Reg: " << reg_num << ", "
 898                   << "Val: " << val
 899                   << ")\n";
 900     }
 901
 902     //------- WRITE REG MASKED ----------//
 903     if(pmt_eq(subp_cmd, s_op_write_reg_masked)) {
 904
 905       long reg_num = pmt_to_long(pmt_nth(0, subp_data));
 906       long val = pmt_to_long(pmt_nth(1, subp_data));
 907       long mask = pmt_to_long(pmt_nth(2, subp_data));
 908
 909       if(!pkt->cs_write_reg_masked(reg_num, val, mask))
 910       {
 911         d_cs_usrp->send(s_cmd_usrp_write,
 912                         pmt_list3(invocation_handle,
 913                                   pmt_from_long(channel),
 914                                   v_packet));
 915
 916         goto new_packet;
 917       }
 918
 919       if(verbose)
 920         std::cout << "[USRP_SERVER] Received write register masked request\n";
 921     }
 922
 923     //------------ READ REG --------------//
 924     if(pmt_eq(subp_cmd, s_op_read_reg)) {
 925
 926       long urid     = pmt_to_long(pmt_nth(0, subp_data));
 927       long reg_num  = pmt_to_long(pmt_nth(1, subp_data));
 928
 929       long srid;
 930       if((srid = next_rid()) == -1)
 931         goto subpkt_bail;
 932
 933       d_rids[srid].owner = port->port_symbol();
 934       d_rids[srid].user_rid = urid;
 935
 936       if(!pkt->cs_read_reg(srid, reg_num))
 937       {
 938         d_cs_usrp->send(s_cmd_usrp_write,
 939                         pmt_list3(invocation_handle,
 940                                   pmt_from_long(channel),
 941                                   v_packet));
 942
 943         // Return the rid
 944         d_rids[srid].owner = PMT_NIL;
 945
 946         goto new_packet;
 947       }
 948
 949       if(verbose)
 950         std::cout << "[USRP_SERVER] Received read register request"
 951                   << " assigning RID " << srid << std::endl;
 952     }
 953
 954     //------------ DELAY --------------//
 955     if(pmt_eq(subp_cmd, s_op_delay)) {
 956
 957       long ticks = pmt_to_long(pmt_nth(0, subp_data));
 958
 959       if(!pkt->cs_delay(ticks))
 960       {
 961         d_cs_usrp->send(s_cmd_usrp_write,
 962                         pmt_list3(invocation_handle,
 963                                   pmt_from_long(channel),
 964                                   v_packet));
 965
 966         goto new_packet;
 967       }
 968
 969       if(verbose)
 970         std::cout << "[USRP_SERVER] Received delay request of "
 971                   << ticks << " ticks\n";
 972     }
 973
 974     //--------- I2C WRITE -----------//
 975     // FIXME: could check that byte count does not exceed 2^8 which
 976     // is the max length in the subpacket for # of bytes to read.
 977     if(pmt_eq(subp_cmd, s_op_i2c_write)) {
 978
 979       long i2c_addr = pmt_to_long(pmt_nth(0, subp_data));
 980       pmt_t data = pmt_nth(1, subp_data);
 981
 982       // Get a readable address to the data which also gives us the length
 983       size_t data_len;
 984       uint8_t *i2c_data = (uint8_t *) pmt_u8vector_writable_elements(data, data_len);
 985
 986       // Make the USB packet
 987       if(!pkt->cs_i2c_write(i2c_addr, i2c_data, data_len))
 988       {
 989         d_cs_usrp->send(s_cmd_usrp_write,
 990                         pmt_list3(invocation_handle,
 991                                   pmt_from_long(channel),
 992                                   v_packet));
 993
 994         goto new_packet;
 995       }
 996
 997       if(verbose)
 998         std::cout << "[USRP_SERVER] Received I2C write\n";
 999     }
1000
1001     //----------- I2C Read -------------//
1002     if(pmt_eq(subp_cmd, s_op_i2c_read)) {
1003
1004       long urid       = pmt_to_long(pmt_nth(0, subp_data));
1005       long i2c_addr   = pmt_to_long(pmt_nth(1, subp_data));
1006       long i2c_bytes  = pmt_to_long(pmt_nth(2, subp_data));
1007
1008       long srid;
1009       if((srid = next_rid()) == -1)
1010         goto subpkt_bail;
1011
1012       d_rids[srid].owner = port->port_symbol();
1013       d_rids[srid].user_rid = urid;
1014
1015       if(!pkt->cs_i2c_read(srid, i2c_addr, i2c_bytes))
1016       {
1017
1018         d_cs_usrp->send(s_cmd_usrp_write,
1019                         pmt_list3(invocation_handle,
1020                                   pmt_from_long(channel),
1021                                   v_packet));
1022
1023         d_rids[srid].owner = PMT_NIL;
1024
1025         goto new_packet;
1026       }
1027
1028       if(verbose)
1029         std::cout << "[USRP_SERVER] Received I2C read\n";
1030     }
1031
1032     //--------- SPI WRITE -----------//
1033     if(pmt_eq(subp_cmd, s_op_spi_write)) {
1034
1035       long enables = pmt_to_long(pmt_nth(0, subp_data));
1036       long format = pmt_to_long(pmt_nth(1, subp_data));
1037       long opt = pmt_to_long(pmt_nth(2, subp_data));
1038       pmt_t data = pmt_nth(3, subp_data);
1039
1040       // Get a readable address to the data which also gives us the length
1041       size_t data_len;
1042       uint8_t *spi_data = (uint8_t *) pmt_u8vector_writable_elements(data, data_len);
1043
1044       // Make the USB packet
1045       if(!pkt->cs_spi_write(enables, format, opt, spi_data, data_len))
1046       {
1047         d_cs_usrp->send(s_cmd_usrp_write,
1048                         pmt_list3(invocation_handle,
1049                                   pmt_from_long(channel),
1050                                   v_packet));
1051
1052         goto new_packet;
1053       }
1054
1055       if(verbose)
1056         std::cout << "[USRP_SERVER] Received SPI write\n";
1057     }
1058
1059     //--------- SPI READ -----------//
1060     if(pmt_eq(subp_cmd, s_op_spi_read)) {
1061
1062       long urid     = pmt_to_long(pmt_nth(0, subp_data));
1063       long enables  = pmt_to_long(pmt_nth(1, subp_data));
1064       long format   = pmt_to_long(pmt_nth(2, subp_data));
1065       long opt      = pmt_to_long(pmt_nth(3, subp_data));
1066       long n_bytes  = pmt_to_long(pmt_nth(4, subp_data));
1067
1068       long srid;
1069       if((srid = next_rid()) == -1)
1070         goto subpkt_bail;
1071
1072       d_rids[srid].owner = port->port_symbol();
1073       d_rids[srid].user_rid = urid;
1074
1075       // Make the USB packet
1076       if(!pkt->cs_spi_read(srid, enables, format, opt, n_bytes))
1077       {
1078         d_cs_usrp->send(s_cmd_usrp_write,
1079                         pmt_list3(invocation_handle,
1080                                   pmt_from_long(channel),
1081                                   v_packet));
1082
1083         // Return the rid
1084         d_rids[srid].owner = PMT_NIL;
1085
1086         goto new_packet;
1087       }
1088
1089       if(verbose)
1090         std::cout << "[USRP_SERVER] Received SPI read\n";
1091     }
1092
1093   subpkt_bail:
1094     curr_subpkt++;
1095
1096   }
1097
1098
1099   // If the current packets length is > 0, we know there are subpackets that
1100   // need to be sent out still.
1101   if(pkt->payload_len() > 0)
1102     d_cs_usrp->send(s_cmd_usrp_write,
1103                     pmt_list3(invocation_handle,
1104                               pmt_from_long(channel),
1105                               v_packet));
1106
1107   return;
1108 }
1109
1110 /*!
1111  * \brief Called by the handle_message() method when the incoming signal is a
1112  * command to start reading samples from the USRP (cmd-start-recv-raw-samples).
1113  *
1114  * The \p port the command was sent on and the channel info (\p chan_info) of
1115  * the channel are passed to ensure that the caller owns the channel.
1116  *
1117  * The \p data parameter should be in the format of a cmd-start-recv-raw-samples
1118  * command where the first element in the list is an invocation handle, and the
1119  * second is the channel the signal generator wants to receive the samples on.
1120  */
1121 void
1122 usrp_server::handle_cmd_start_recv_raw_samples(
1123                                   mb_port_sptr port,
1124                                   std::vector<struct channel_info> &chan_info,
1125                                   pmt_t data)
1126 {
1127   pmt_t invocation_handle = pmt_nth(0, data);
1128   long channel = pmt_to_long(pmt_nth(1, data));
1129
1130   // Ensure the channel is valid and the caller owns the port
1131   if(!check_valid(port, channel, chan_info,
1132                   pmt_list2(s_response_xmit_raw_frame, invocation_handle)))
1133     return;
1134
1135   // Already started receiving samples? (another start before a stop)
1136   // Check the RX channel bitmask.
1137   if(d_rx_chan_mask & (1 << channel)) {
1138     port->send(s_response_recv_raw_samples,
1139                pmt_list5(invocation_handle,
1140                          s_err_already_receiving,
1141                          PMT_NIL,
1142                          PMT_NIL,
1143                          PMT_NIL));
1144     return;
1145   }
1146
1147   // We only need to generate a 'start reading' command down to the
1148   // low level interface if no other channel is already reading
1149   //
1150   // We carry this over the CS interface because the lower level
1151   // interface does not care about the channel, we only demux it
1152   // at the usrp_server on responses.
1153   if(d_rx_chan_mask == 0) {
1154
1155     if(verbose)
1156       std::cout << "[USRP_SERVER] Sending read request down to start recv\n";
1157
1158     d_cs_usrp->send(s_cmd_usrp_start_reading, pmt_list1(invocation_handle));
1159   }
1160
1161   d_rx_chan_mask |= 1<<channel;
1162
1163   return;
1164 }
1165
1166 /*!
1167  * \brief Called by the handle_message() method when the incoming signal is to
1168  * stop receiving samples from the USRP (cmd-stop-recv-raw-samples).
1169  *
1170  * The \p port the command was sent on and the channel info (\p chan_info) of
1171  * the channel are passed to ensure that the caller owns the channel.
1172  *
1173  * The \p data parameter should be in the format of a cmd-stop-recv-raw-samples
1174  * command where the first element in the list is an invocation handle, and the
1175  * second is the channel the signal generator wants to stop receiving the
1176  * samples from.
1177  */
1178 void
1179 usrp_server::handle_cmd_stop_recv_raw_samples(
1180                         mb_port_sptr port,
1181                         std::vector<struct channel_info> &chan_info,
1182                         pmt_t data)
1183 {
1184   pmt_t invocation_handle = pmt_nth(0, data);
1185   long channel = pmt_to_long(pmt_nth(1, data));
1186
1187   // FIX ME : we have no responses to send an error...
1188   // Ensure the channel is valid and the caller owns the port
1189   //if(!check_valid(port, channel, chan_info,
1190   //                pmt_list2(s_response_xmit_raw_frame, invocation_handle)))
1191   //  return;
1192
1193   // Remove this hosts bit from the receiver mask
1194   d_rx_chan_mask &= ~(1<<channel);
1195
1196   // We only need to generate a 'start reading' command down to the
1197   // low level interface if no other channel is already reading
1198   //
1199   // We carry this over the CS interface because the lower level
1200   // interface does not care about the channel, we only demux it
1201   // at the usrp_server on responses.
1202   if(d_rx_chan_mask == 0) {
1203
1204     if(verbose)
1205       std::cout << "[USRP_SERVER] Sending stop reading request down\n";
1206
1207     d_cs_usrp->send(s_cmd_usrp_stop_reading, pmt_list1(invocation_handle));
1208   }
1209
1210   return;
1211 }
1212
1213 /*!
1214  * \brief Called by the handle_message() method when an incoming signal is
1215  * generated to USRP server that contains raw samples from the USRP.  This
1216  * method generates the response-recv-raw-samples signals that are the result of
1217  * a cmd-start-recv-raw-samples signal.
1218  *
1219  * The raw lower-level packet is extracted from \p data, where the format for \p
1220  * data is a PMT list.  The PMT \p data list should contain an invocation handle
1221  * as the first element, the status of the lower-level read as the second
1222  * element, and a uniform vector representation of the packets as the third
1223  * element.
1224  *
1225  * The packet contains a channel field that the samples are destined to, and the
1226  * method determines where to send the samples based on this channel since each
1227  * channel has an associated port which allocated it.
1228  */
1229 void
1230 usrp_server::handle_response_usrp_read(pmt_t data)
1231 {
1232
1233   pmt_t invocation_handle = pmt_nth(0, data);
1234   pmt_t status = pmt_nth(1, data);
1235   pmt_t v_pkt = pmt_nth(2, data);
1236
1237   size_t n_bytes;
1238   size_t ignore;
1239
1240   if (d_fake_rx) {
1241
1242     pmt_t pkt = pmt_nth(2, data);
1243
1244     d_rx[0]->send(s_response_recv_raw_samples,
1245                   pmt_list5(PMT_F,
1246                             PMT_T,
1247                             pkt,
1248                             pmt_from_long(0xffff),
1249                             PMT_NIL));
1250
1251     return;
1252   }
1253
1254   // Extract the packet and return appropriately
1255   transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_pkt, n_bytes);
1256
1257   // The channel is used to find the port to pass the samples on
1258   long channel = pkt->chan();
1259   long payload_len = pkt->payload_len();
1260   long port;
1261
1262   // Ignore packets which seem to have incorrect size or size 0
1263   if(payload_len > pkt->max_payload() || payload_len == 0)
1264     return;
1265
1266   // If the packet is a C/S packet, parse it separately
1267   if(channel == CONTROL_CHAN) {
1268     parse_control_pkt(invocation_handle, pkt);
1269     return;
1270   }
1271
1272   if((port = rx_port_index(d_chaninfo_rx[channel].owner)) == -1)
1273     return; // Don't know where to send the sample... possibility on abrupt close
1274
1275   pmt_t v_samples = pmt_make_u8vector(payload_len, 0);
1276   uint8_t *samples = pmt_u8vector_writable_elements(v_samples, ignore);
1277
1278   memcpy(samples, pkt->payload(), payload_len);
1279
1280   // Build a properties dictionary to store things such as the RSSI
1281   pmt_t properties =  pmt_make_dict();
1282
1283   pmt_dict_set(properties,
1284                pmt_intern("rssi"),
1285                pmt_from_long(pkt->rssi()));
1286
1287   if(pkt->overrun())
1288     pmt_dict_set(properties,
1289                  pmt_intern("overrun"),
1290                  PMT_T);
1291
1292   if(pkt->underrun())
1293     pmt_dict_set(properties,
1294                  pmt_intern("underrun"),
1295                  PMT_T);
1296
1297   d_rx[port]->send(s_response_recv_raw_samples,
1298                    pmt_list6(invocation_handle,
1299                              status,
1300                              v_samples,
1301                              pmt_from_long(pkt->timestamp()),
1302                              pmt_from_long(channel),
1303                              properties));
1304   return;
1305 }
1306
1307 /*!
1308  * \brief Called by handle_response_usrp_read() when the incoming packet has a
1309  * channel of CONTROL_CHAN.  This means that the incoming packet contains a
1310  * response for a command sent to the control channel, which this method will
1311  * parse.
1312  *
1313  * The \p pkt parameter is a pointer to the full packet (transport_pkt) in
1314  * memory.
1315  *
1316  * Given that all commands sent to the control channel that require responses
1317  * will carry an RID (request ID), the method will use the RID passed back with
1318  * the response to determine which port the response should be sent on.
1319  */
1320 void
1321 usrp_server::parse_control_pkt(pmt_t invocation_handle, transport_pkt *pkt)
1322 {
1323
1324   long payload_len = pkt->payload_len();
1325   long curr_payload = 0;
1326   long port;
1327
1328   // We dispatch based on the control packet type, however we can extract the
1329   // opcode and the length immediately which is consistent in all responses.
1330   //
1331   // Since each control packet can have multiple responses, we keep reading the
1332   // lengths of each subpacket until we reach the payload length.
1333   while(curr_payload < payload_len) {
1334
1335     pmt_t sub_packet = pkt->read_subpacket(curr_payload);
1336     pmt_t op_symbol = pmt_nth(0, sub_packet);
1337
1338     int len = pkt->cs_len(curr_payload);
1339
1340     if(verbose)
1341       std::cout << "[USRP_SERVER] Parsing subpacket "
1342                 << op_symbol << " ... length " << len << std::endl;
1343
1344     //----------------- PING RESPONSE ------------------//
1345     if(pmt_eq(op_symbol, s_op_ping_fixed_reply)) {
1346
1347       long srid     = pmt_to_long(pmt_nth(1, sub_packet));
1348       pmt_t pingval = pmt_nth(2, sub_packet);
1349
1350       long urid = d_rids[srid].user_rid;
1351
1352       if(verbose)
1353         std::cout << "[USRP_SERVER] Found ping response "
1354                   << "("
1355                   << "URID: " << urid << ", "
1356                   << "SRID: " << srid << ", "
1357                   << "VAL: " << pingval
1358                   << ")\n";
1359
1360       // Do some bounds checking incase of bogus/corrupt responses
1361       if(srid > D_MAX_RID)
1362         return;
1363
1364       pmt_t owner = d_rids[srid].owner;
1365
1366       // Return the RID
1367       d_rids[srid].owner = PMT_NIL;
1368
1369       // FIXME: should be 1 response for all subpackets here ?
1370       if((port = tx_port_index(owner)) != -1)
1371         d_tx[port]->send(s_response_from_control_channel,
1372                          pmt_list4(invocation_handle,
1373                                    PMT_T,
1374                                    pmt_list2(s_op_ping_fixed_reply, // subp
1375                                              pmt_list2(pmt_from_long(urid),
1376                                                        pingval)),
1377                                    pmt_from_long(pkt->timestamp())));
1378     }
1379
1380     //----------------- READ REG RESPONSE ------------------//
1381     else if(pmt_eq(op_symbol, s_op_read_reg_reply)) {
1382
1383       long srid     = pmt_to_long(pmt_nth(1, sub_packet));
1384       pmt_t reg_num = pmt_nth(2, sub_packet);
1385       pmt_t reg_val = pmt_nth(3, sub_packet);
1386
1387       long urid = d_rids[srid].user_rid;
1388
1389       if(verbose)
1390         std::cout << "[USRP_SERVER] Found read register response "
1391                   << "("
1392                   << "URID: " << urid << ", "
1393                   << "SRID: " << srid << ", "
1394                   << "REG: " << reg_num << ", "
1395                   << "VAL: " << reg_val
1396                   << ")\n";
1397
1398       // Do some bounds checking to avoid seg faults
1399       if(srid > D_MAX_RID)
1400         return;
1401
1402       pmt_t owner = d_rids[srid].owner;
1403
1404       // Return the RID
1405       d_rids[srid].owner = PMT_NIL;
1406
1407       // FIXME: should be 1 response for all subpackets here ?
1408       if((port = tx_port_index(owner)) != -1)
1409         d_tx[port]->send(s_response_from_control_channel,
1410                          pmt_list4(invocation_handle,
1411                                    PMT_T,
1412                                    pmt_list2(s_op_read_reg_reply, // subp
1413                                              pmt_list3(pmt_from_long(urid),
1414                                                        reg_num,
1415                                                        reg_val)),
1416                                    pmt_from_long(pkt->timestamp())));
1417     }
1418
1419     //------------------ I2C READ REPLY -------------------//
1420     else if(pmt_eq(op_symbol, s_op_i2c_read_reply)) {
1421
1422       long srid       = pmt_to_long(pmt_nth(1, sub_packet));
1423       pmt_t i2c_addr  = pmt_nth(2, sub_packet);
1424       pmt_t i2c_data  = pmt_nth(3, sub_packet);
1425
1426       long urid = d_rids[srid].user_rid;
1427
1428       if(verbose)
1429         std::cout << "[USRP_SERVER] Found i2c read reply "
1430                   << "("
1431                   << "URID: " << urid << ", "
1432                   << "SRID: " << srid << ", "
1433                   << "Addr: " << i2c_addr << ", "
1434                   << "Data: " << i2c_data
1435                   << ")\n";
1436
1437       // Do some bounds checking to avoid seg faults
1438       if(srid > D_MAX_RID)
1439         return;
1440
1441       pmt_t owner = d_rids[srid].owner;
1442
1443       // Return the RID
1444       d_rids[srid].owner = PMT_NIL;
1445
1446       if((port = tx_port_index(owner)) != -1)
1447         d_tx[port]->send(s_response_from_control_channel,
1448                          pmt_list4(invocation_handle,
1449                                    PMT_T,
1450                                    pmt_list2(s_op_i2c_read_reply,
1451                                              pmt_list3(pmt_from_long(urid),
1452                                                        i2c_addr,
1453                                                        i2c_data)),
1454                                    pmt_from_long(pkt->timestamp())));
1455     }
1456
1457     //------------------ SPI READ REPLY -------------------//
1458     else if(pmt_eq(op_symbol, s_op_spi_read_reply)) {
1459
1460       long srid       = pmt_to_long(pmt_nth(1, sub_packet));
1461       pmt_t spi_data  = pmt_nth(2, sub_packet);
1462
1463       long urid = d_rids[srid].user_rid;
1464
1465       if(verbose)
1466         std::cout << "[USRP_SERVER] Found SPI read reply "
1467                   << "("
1468                   << "URID: " << urid << ", "
1469                   << "SRID: " << srid << ", "
1470                   << "Data: " << spi_data
1471                   << ")\n";
1472
1473       // Bounds check the RID
1474       if(srid > D_MAX_RID)
1475         return;
1476
1477       pmt_t owner = d_rids[srid].owner;
1478
1479       // Return the RID
1480       d_rids[srid].owner = PMT_NIL;
1481
1482       if((port = tx_port_index(owner)) != -1)
1483         d_tx[port]->send(s_response_from_control_channel,
1484                          pmt_list4(invocation_handle,
1485                                    PMT_T,
1486                                    pmt_list2(s_op_spi_read_reply,
1487                                              pmt_list2(pmt_from_long(urid),
1488                                                        spi_data)),
1489                                    pmt_from_long(pkt->timestamp())));
1490     }
1491
1492     // Each subpacket has an unaccounted for 2 bytes which is the opcode
1493     // and the length field
1494     curr_payload += len + 2;
1495
1496     // All subpackets are 32-bit aligned
1497     int align_offset = 4 - (curr_payload % 4);
1498
1499     if(align_offset != 4)
1500       curr_payload += align_offset;
1501   }
1502 }
1503
1504 /*!
1505  * \brief Used to recall all incoming signals that were deferred when USRP
1506  * server was in the initialization state.
1507  */
1508 void
1509 usrp_server::recall_defer_queue()
1510 {
1511
1512   std::vector<mb_message_sptr> recall;
1513
1514   while(!d_defer_queue.empty()) {
1515     recall.push_back(d_defer_queue.front());
1516     d_defer_queue.pop();
1517   }
1518
1519   // Parse the messages that were queued while waiting for an open response
1520   for(int i=0; i < (int)recall.size(); i++)
1521     handle_message(recall[i]);
1522
1523   return;
1524 }
1525
1526 /*!
1527  * \brief Commonly called by any method which handles outgoing frames or control
1528  * packets to the USRP to check if the port on which the signal was sent owns
1529  * the channel the outgoing packet will be associated with.   This helps ensure
1530  * that applications do not send data on other application's ports.
1531  *
1532  * The \p port parameter is the port symbol that the caller wishes to determine
1533  * owns the channel specified by \p chan_info.
1534  *
1535  * The \p signal_info parameter is a PMT list containing two elements: the
1536  * response signal to use if the permissions are invalid, and the invocation
1537  * handle that was passed.  This allows the method to generate detailed failure
1538  * responses to signals without having to return some sort of structured
1539  * information which the caller must then parse and interpret to determine the
1540  * failure type.
1541  *
1542  * \returns true if \p port owns the channel specified by \p chan_info, false
1543  * otherwise.
1544  */
1545 bool
1546 usrp_server::check_valid(mb_port_sptr port,
1547                          long channel,
1548                          std::vector<struct channel_info> &chan_info,
1549                          pmt_t signal_info)
1550 {
1551
1552   pmt_t response_signal = pmt_nth(0, signal_info);
1553   pmt_t invocation_handle = pmt_nth(1, signal_info);
1554
1555   // not a valid channel number?
1556   if(channel >= (long)chan_info.size() && channel != CONTROL_CHAN) {
1557     port->send(response_signal,
1558                pmt_list2(invocation_handle,
1559                          s_err_channel_invalid));
1560
1561     if(verbose)
1562       std::cout << "[USRP_SERVER] Invalid channel number for event "
1563                 << response_signal << std::endl;
1564     return false;
1565   }
1566
1567   // not the owner of the port?
1568   if(chan_info[channel].owner != port->port_symbol()) {
1569     port->send(response_signal,
1570                pmt_list2(invocation_handle,
1571                          s_err_channel_permission_denied));
1572
1573     if(verbose)
1574       std::cout << "[USRP_SERVER] Invalid permissions"
1575                 << " for " << response_signal
1576                 << " from " << port->port_symbol()
1577                 << " proper owner is " << chan_info[channel].owner
1578                 << " on channel " << channel
1579                 << " invocation " << invocation_handle
1580                 << std::endl;
1581     return false;
1582   }
1583
1584   return true;
1585 }
1586
1587 /*!
1588  * \brief Finds the next available RID for internal USRP server use with control
1589  * and status packets.
1590  *
1591  * \returns the next valid RID or -1 if no more RIDs are available.
1592  */
1593 long
1594 usrp_server::next_rid()
1595 {
1596   for(int i = 0; i < D_MAX_RID; i++)
1597     if(pmt_eqv(d_rids[i].owner, PMT_NIL))
1598       return i;
1599
1600   if(verbose)
1601     std::cout << "[USRP_SERVER] No RIDs left\n";
1602   return -1;
1603 }
1604
1605 /*!
1606  * \brief Called by handle_message() when USRP server gets a response that the
1607  * USRP was opened successfully to initialize the registers using the new
1608  * register read/write control packets.
1609  */
1610 void
1611 usrp_server::initialize_registers()
1612 {
1613   // We use handle_cmd_to_control_channel() to create the register writes using
1614   // PMT_NIL as the response port to tell usrp_server not to pass the response
1615   // up to any application.
1616   if(verbose)
1617     std::cout << "[USRP_SERVER] Initializing registers...\n";
1618
1619   // RX mode to normal (0)
1620   set_register(FR_MODE, 0);
1621
1622   // FPGA debugging?
1623   if(d_fpga_debug) {
1624     set_register(FR_DEBUG_EN, 1);
1625     // FIXME: need to figure out exact register writes to control daughterboard
1626     // pins that need to be written to
1627   } else {
1628     set_register(FR_DEBUG_EN, 0);
1629   }
1630
1631   // Set the transmit sample rate divisor, which is 4-1
1632   set_register(FR_TX_SAMPLE_RATE_DIV, 3);
1633
1634   // Dboard IO buffer and register settings
1635   set_register(FR_OE_0, (0xffff << 16) | 0x0000);
1636   set_register(FR_IO_0, (0xffff << 16) | 0x0000);
1637   set_register(FR_OE_1, (0xffff << 16) | 0x0000);
1638   set_register(FR_IO_1, (0xffff << 16) | 0x0000);
1639   set_register(FR_OE_2, (0xffff << 16) | 0x0000);
1640   set_register(FR_IO_2, (0xffff << 16) | 0x0000);
1641   set_register(FR_OE_3, (0xffff << 16) | 0x0000);
1642   set_register(FR_IO_3, (0xffff << 16) | 0x0000);
1643
1644   // zero Tx side Auto Transmit/Receive regs
1645   set_register(FR_ATR_MASK_0, 0);
1646   set_register(FR_ATR_TXVAL_0, 0);
1647   set_register(FR_ATR_RXVAL_0, 0);
1648   set_register(FR_ATR_MASK_1, 0);
1649   set_register(FR_ATR_TXVAL_1, 0);
1650   set_register(FR_ATR_RXVAL_1, 0);
1651   set_register(FR_ATR_MASK_2, 0);
1652   set_register(FR_ATR_TXVAL_2, 0);
1653   set_register(FR_ATR_RXVAL_2, 0);
1654   set_register(FR_ATR_MASK_3, 0);
1655   set_register(FR_ATR_TXVAL_3, 0);
1656   set_register(FR_ATR_RXVAL_3, 0);
1657
1658   // Configure TX mux, this is a hacked value
1659   set_register(FR_TX_MUX, 0x00000081);
1660
1661   // Set the interpolation rate, which is the rate divided by 4, minus 1
1662   set_register(FR_INTERP_RATE, (d_interp_tx/4)-1);
1663
1664   // Apparently this register changes again
1665   set_register(FR_TX_MUX, 0x00000981);
1666
1667   // Set the receive sample rate divisor, which is 2-1
1668   set_register(FR_RX_SAMPLE_RATE_DIV, 1);
1669
1670   // DC offset
1671   set_register(FR_DC_OFFSET_CL_EN, 0x0000000f);
1672
1673   // Reset the DC correction offsets
1674   set_register(FR_ADC_OFFSET_0, 0);
1675   set_register(FR_ADC_OFFSET_1, 0);
1676
1677   // Some hard-coded RX configuration
1678   set_register(FR_RX_FORMAT, 0x00000300);
1679   set_register(FR_RX_MUX, 1);
1680
1681   // RX decimation rate is divided by two, then subtract 1
1682   set_register(FR_DECIM_RATE, (d_decim_rx/2)-1);
1683
1684   // More hard coding
1685   set_register(FR_RX_MUX, 0x000e4e41);
1686
1687   // Resetting RX registers
1688   set_register(FR_RX_PHASE_0, 0);
1689   set_register(FR_RX_PHASE_1, 0);
1690   set_register(FR_RX_PHASE_2, 0);
1691   set_register(FR_RX_PHASE_3, 0);
1692   set_register(FR_RX_FREQ_0, 0x28000000);
1693   set_register(FR_RX_FREQ_1, 0);
1694   set_register(FR_RX_FREQ_2, 0);
1695   set_register(FR_RX_FREQ_3, 0);
1696
1697   // Enable debug bus
1698   set_register(FR_DEBUG_EN, 0xf);
1699   set_register(FR_OE_0, -1);
1700   set_register(FR_OE_1, -1);
1701   set_register(FR_OE_2, -1);
1702   set_register(FR_OE_3, -1);
1703
1704   // DEBUGGING
1705   //check_register_initialization();
1706 }
1707
1708 // FIXME: used for debugging to determine if all the registers are actually
1709 // being set correctly
1710 void
1711 usrp_server::check_register_initialization()
1712 {
1713   // RX mode to normal (0)
1714   read_register(FR_MODE);
1715
1716   // FPGA debugging?
1717   if(d_fpga_debug) {
1718     read_register(FR_DEBUG_EN);
1719     // FIXME: need to figure out exact register writes to control daughterboard
1720     // pins that need to be written to
1721   } else {
1722     read_register(FR_DEBUG_EN);
1723   }
1724
1725   // Set the transmit sample rate divisor, which is 4-1
1726   read_register(FR_TX_SAMPLE_RATE_DIV);
1727
1728   // Dboard IO buffer and register settings
1729   read_register(FR_OE_0);
1730   read_register(FR_IO_0);
1731   read_register(FR_OE_1);
1732   read_register(FR_IO_1);
1733   read_register(FR_OE_2);
1734   read_register(FR_IO_2);
1735   read_register(FR_OE_3);
1736   read_register(FR_IO_3);
1737
1738   // zero Tx side Auto Transmit/Receive regs
1739   read_register(FR_ATR_MASK_0);
1740   read_register(FR_ATR_TXVAL_0);
1741   read_register(FR_ATR_RXVAL_0);
1742   read_register(FR_ATR_MASK_1);
1743   read_register(FR_ATR_TXVAL_1);
1744   read_register(FR_ATR_RXVAL_1);
1745   read_register(FR_ATR_MASK_2);
1746   read_register(FR_ATR_TXVAL_2);
1747   read_register(FR_ATR_RXVAL_2);
1748   read_register(FR_ATR_MASK_3);
1749   read_register(FR_ATR_TXVAL_3);
1750   read_register(FR_ATR_RXVAL_3);
1751
1752   // Configure TX mux, this is a hacked value
1753   read_register(FR_TX_MUX);
1754
1755   // Set the interpolation rate, which is the rate divided by 4, minus 1
1756   read_register(FR_INTERP_RATE);
1757
1758   // Apparently this register changes again
1759   read_register(FR_TX_MUX);
1760
1761   // Set the receive sample rate divisor, which is 2-1
1762   read_register(FR_RX_SAMPLE_RATE_DIV);
1763
1764   // DC offset
1765   read_register(FR_DC_OFFSET_CL_EN);
1766
1767   // Reset the DC correction offsets
1768   read_register(FR_ADC_OFFSET_0);
1769   read_register(FR_ADC_OFFSET_1);
1770
1771   // Some hard-coded RX configuration
1772   read_register(FR_RX_FORMAT);
1773   read_register(FR_RX_MUX);
1774
1775   // RX decimation rate is divided by two, then subtract 1
1776   read_register(FR_DECIM_RATE);
1777
1778   // More hard coding
1779   read_register(FR_RX_MUX);
1780
1781   // Resetting RX registers
1782   read_register(FR_RX_PHASE_0);
1783   read_register(FR_RX_PHASE_1);
1784   read_register(FR_RX_PHASE_2);
1785   read_register(FR_RX_PHASE_3);
1786   read_register(FR_RX_FREQ_0);
1787   read_register(FR_RX_FREQ_1);
1788   read_register(FR_RX_FREQ_2);
1789   read_register(FR_RX_FREQ_3);
1790 }
1791
1792 /*!
1793  * \brief Used to generate FPGA register write commands to reset all of the FPGA
1794  * registers to a value of 0.
1795  */
1796 void
1797 usrp_server::reset_all_registers()
1798 {
1799   for(int i=0; i<64; i++)
1800     set_register(i, 0);
1801 }
1802
1803 /*!
1804  * \brief Used internally by USRP server to generate a control/status packet
1805  * which contains a register write.
1806  *
1807  * The \p reg parameter is the register number that the value \p val will be
1808  * written to.
1809  */
1810 void
1811 usrp_server::set_register(long reg, long val)
1812 {
1813   size_t psize;
1814   long payload_len = 0;
1815
1816   pmt_t v_packet = pmt_make_u8vector(sizeof(transport_pkt), 0);
1817   transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_packet, psize);
1818
1819   pkt->set_header(0, CONTROL_CHAN, 0, payload_len);
1820   pkt->set_timestamp(0xffffffff);
1821
1822   pkt->cs_write_reg(reg, val);
1823
1824   d_cs_usrp->send(s_cmd_usrp_write,
1825                   pmt_list3(PMT_NIL,
1826                             pmt_from_long(CONTROL_CHAN),
1827                             v_packet));
1828 }
1829
1830 /*!
1831  * \brief Used internally by USRP server to generate a control/status packet
1832  * which contains a register read.  This is important to use internally so that
1833  * USRP server can bypass the use of RIDs with register reads, as they are not
1834  * needed and it would use up the finite number of RIDs available for use for
1835  * applications to receive responses.
1836  *
1837  * The \p reg parameter is the register number that the value should be read
1838  * from.
1839  */
1840 void
1841 usrp_server::read_register(long reg)
1842 {
1843   size_t psize;
1844   long payload_len = 0;
1845
1846   pmt_t v_packet = pmt_make_u8vector(sizeof(transport_pkt), 0);
1847   transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_packet, psize);
1848
1849   pkt->set_header(0, CONTROL_CHAN, 0, payload_len);
1850   pkt->set_timestamp(0xffffffff);
1851
1852   pkt->cs_read_reg(0, reg);
1853
1854   d_cs_usrp->send(s_cmd_usrp_write,
1855                   pmt_list3(PMT_NIL,
1856                             pmt_from_long(CONTROL_CHAN),
1857                             v_packet));
1858 }
1859
1860 REGISTER_MBLOCK_CLASS(usrp_server);