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