/* -*- c++ -*- */
/*
 * Copyright 2007,2008 Free Software Foundation, Inc.
 * 
 * This file is part of GNU Radio
 * 
 * GNU Radio is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3, or (at your option)
 * any later version.
 * 
 * GNU Radio is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License along
 * with this program; if not, write to the Free Software Foundation, Inc.,
 * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <usrp_server.h>
#include <iostream>
#include <usrp_inband_usb_packet.h>
#include <mblock/class_registry.h>
#include <vector>
#include <usrp_usb_interface.h>
#include <string.h>
#include <fpga_regs_common.h>
#include <fpga_regs_standard.h>

#include <symbols_usrp_server_cs.h>
#include <symbols_usrp_channel.h>
#include <symbols_usrp_tx.h>
#include <symbols_usrp_rx.h>
#include <symbols_usrp_low_level_cs.h>
#include <symbols_usrp_interface_cs.h>

static pmt_t s_shutdown = pmt_intern("%shutdown");

typedef usrp_inband_usb_packet transport_pkt;   // makes conversion to gigabit easy

const static bool verbose = false;

static std::string
str(long x)
{
  std::ostringstream s;
  s << x;
  return s.str();
}

usrp_server::usrp_server(mb_runtime *rt, const std::string &instance_name, pmt_t user_arg)
  : mb_mblock(rt, instance_name, user_arg),
  d_fpga_debug(false),
  d_interp_tx(128),     // these should match the lower level defaults (rx also)
  d_decim_rx(128),
  d_fake_rx(false)
{
  if(verbose)
    std::cout << "[USRP_SERVER] Initializing...\n";

  // Dictionary for arguments to all of the components
  d_usrp_dict = user_arg;
  
  if (pmt_is_dict(d_usrp_dict)) {

    if(pmt_t fpga_debug = pmt_dict_ref(d_usrp_dict, 
                                      pmt_intern("fpga-debug"), 
                                      PMT_NIL)) {
      if(pmt_eqv(fpga_debug, PMT_T)) 
        d_fpga_debug=true;
    }
    
    // Read the TX interpolations
    if(pmt_t interp_tx = pmt_dict_ref(d_usrp_dict, 
                                      pmt_intern("interp-tx"), 
                                      PMT_NIL)) {
      if(!pmt_eqv(interp_tx, PMT_NIL)) 
        d_interp_tx = pmt_to_long(interp_tx);
    }
    
    // Read the RX decimation rate
    if(pmt_t decim_rx = pmt_dict_ref(d_usrp_dict, 
                                      pmt_intern("decim-rx"), 
                                      PMT_NIL)) {
      if(!pmt_eqv(decim_rx, PMT_NIL)) 
        d_decim_rx = pmt_to_long(decim_rx);
    }
  }
  
  // control & status port
  d_cs = define_port("cs", "usrp-server-cs", true, mb_port::EXTERNAL);	
  d_cs_usrp = define_port("cs_usrp", "usrp-interface-cs", false, mb_port::INTERNAL);	

  // ports
  //
  // (if/when we do replicated ports, these will be replaced by a
  //  single replicated port)
  for(int port=0; port < N_PORTS; port++) {

    d_tx.push_back(define_port("tx"+str(port), 
                               "usrp-tx", 
                               true, 
                               mb_port::EXTERNAL));

    d_rx.push_back(define_port("rx"+str(port), 
                               "usrp-rx", 
                               true, 
                               mb_port::EXTERNAL));
  }

  define_component("usrp", "usrp_usb_interface", d_usrp_dict);
  connect("self", "cs_usrp", "usrp", "cs");

  d_defer=false;
  d_opened=false;

  // FIXME: needs to be returned from open, if we want to use this
  d_nrx_chan = 2;
  d_ntx_chan = 2;

  // Initialize capacity on each channel to 0 and to no owner
  // Also initialize the USRP standard tx/rx pointers to NULL
  for(int chan=0; chan < d_ntx_chan; chan++)
    d_chaninfo_tx.push_back(channel_info());

  for(int chan=0; chan < d_nrx_chan; chan++)
    d_chaninfo_rx.push_back(channel_info());

  d_rx_chan_mask = 0;

  for(int i=0; i < D_MAX_RID; i++) 
    d_rids.push_back(rid_info());

  //d_fake_rx=true;
}

/*!
 * \brief resets the assigned capacity and owners of each RX and TX channel from
 * allocations.
 */
void
usrp_server::reset_channels()
{

  for(int chan=0; chan < d_ntx_chan; chan++) {
    d_chaninfo_tx[chan].assigned_capacity = 0;
    d_chaninfo_tx[chan].owner = PMT_NIL;
  }

  for(int chan=0; chan < d_nrx_chan; chan++) {
    d_chaninfo_rx[chan].assigned_capacity = 0;
    d_chaninfo_rx[chan].owner = PMT_NIL;
  }

  d_rx_chan_mask = 0;
}

usrp_server::~usrp_server()
{
}


void
usrp_server::initial_transition()
{
  // the initial transition
}

/*!
 * \brief Reads all incoming messages to USRP server from the TX, RX, and the CS
 * ports.  This drives the state of USRP server and dispatches based on the
 * message.
 */
void
usrp_server::handle_message(mb_message_sptr msg)
{
  pmt_t event = msg->signal();		// the "name" of the message
  pmt_t port_id = msg->port_id();	// which port it came in on
  pmt_t data = msg->data();
  pmt_t invocation_handle;
  pmt_t metadata = msg->metadata();
  pmt_t status;

  long port;

  if (pmt_eq(event, s_shutdown))	// ignore (for now)
    return;

  invocation_handle = pmt_nth(0, data);

  if (0){
    std::cout << "[USRP_SERVER] event: " << event << std::endl;
    std::cout << "[USRP_SERVER] port_id: " << port_id << std::endl;
  }

  // It would be nice if this were all table driven, and we could compute our
  // state transition as f(current_state, port_id, signal)
  
  // A message from the USRP CS, which should *only* be responses
  //
  // It is important that this set come before checking messages of any other
  // components.  This is since we always want to listen to the low level USRP
  // server, even if we aren't initialized we are waiting for responses to
  // become initialized.  Likewise, after the usrp_server is "closed", we still
  // want to pass responses back from the low level.

  //---------------- USRP RESPONSE ---------------//
  if (pmt_eq(port_id, d_cs_usrp->port_symbol())) { 
    
    //-------------- USRP OPEN ------------------//
    if(pmt_eq(event, s_response_usrp_open)) {
      // pass the response back over the regular CS port
      pmt_t status = pmt_nth(1, data);
      d_cs->send(s_response_open, pmt_list2(invocation_handle, status));

      //reset_all_registers();
      //initialize_registers();

      if(pmt_eqv(status,PMT_T)) {
        d_opened = true;
        d_defer = false;
        recall_defer_queue();
      }

      return;
    }
    //------------- USRP CLOSE -------------------//
    else if (pmt_eq(event, s_response_usrp_close)) {
      pmt_t status = pmt_nth(1, data);
      d_cs->send(s_response_close, pmt_list2(invocation_handle, status));

      if(pmt_eqv(status,PMT_T)) {
        d_opened = false;
        d_defer = false;
        reset_channels();
        recall_defer_queue();
      }
      
      return;
    }
    //--------------- USRP WRITE --------------//
    else if (pmt_eq(event, s_response_usrp_write)) {
      
      pmt_t status = pmt_nth(1, data);
      long channel = pmt_to_long(pmt_nth(2, data));
      long port;

      // Do not report back responses if they were generated from a
      // command packet
      if(channel == CONTROL_CHAN)
        return;

      // Find the port through the owner of the channel
      if((port = tx_port_index(d_chaninfo_tx[channel].owner)) !=-1 ){
        d_tx[port]->send(s_response_xmit_raw_frame, 
                         pmt_list2(invocation_handle, status));
	return;
      }
    }
    //--------------- USRP READ ---------------//
    else if (pmt_eq(event, s_response_usrp_read)) {

      pmt_t status = pmt_nth(1, data);

      if(!pmt_eqv(status, PMT_T)) {
        std::cerr << "[USRP_SERVER] Error receiving packet\n";
        return;
      }
      else {
        handle_response_usrp_read(data);
        return;
      }
    }

    goto unhandled;
  }

  // Checking for defer on all other messages
  if(d_defer) {
    if (verbose)
      std::cout << "[USRP_SERVER] Received msg while deferring (" 
                << msg->signal() << ")\n";
    d_defer_queue.push(msg);
    return;
  }
  
  //--------- CONTROL / STATUS ------------//
  if (pmt_eq(port_id, d_cs->port_symbol())){
    
    //----------- OPEN -----------//
    if (pmt_eq(event, s_cmd_open)){

      // Reject if already open
      if(d_opened) {
        d_cs->send(s_response_open, pmt_list2(invocation_handle, s_err_usrp_already_opened));
        return;
      }

      // the parameters are the same to the low level interface, so we just pass 'data' along
      d_cs_usrp->send(s_cmd_usrp_open, data);

      d_defer = true;
      
      return;
    }
    //---------- CLOSE -----------//
    else if (pmt_eq(event, s_cmd_close)){
      
      if(!d_opened) { 
        d_cs->send(s_response_close, pmt_list2(invocation_handle, s_err_usrp_already_closed));
        return;
      }
      
      d_defer = true;
      d_cs_usrp->send(s_cmd_usrp_close, pmt_list1(invocation_handle));

      return;
    }
    //---------- MAX CAPACITY ----------//
    else if (pmt_eq(event, s_cmd_max_capacity)) {
      
      if(!d_opened) { 
        d_cs->send(s_response_max_capacity, 
                   pmt_list3(invocation_handle, s_err_usrp_not_opened, pmt_from_long(0)));
        return;
      }

      d_cs->send(s_response_max_capacity, 
                 pmt_list3(invocation_handle, 
                           PMT_T, 
                           pmt_from_long(max_capacity())));
      return;
    }
    //---------- NTX CHAN --------------//
    else if (pmt_eq(event, s_cmd_ntx_chan)) {

      if(!d_opened) { 
        d_cs->send(s_response_ntx_chan, 
                   pmt_list3(invocation_handle, s_err_usrp_not_opened, pmt_from_long(0)));
        return;
      }

      d_cs->send(s_response_ntx_chan, 
                 pmt_list3(invocation_handle, 
                           PMT_T, 
                           pmt_from_long(d_ntx_chan)));
      return;
    }
    //---------- NRX CHAN -----------//
    else if (pmt_eq(event, s_cmd_nrx_chan)) {

      if(!d_opened) { 
        d_cs->send(s_response_nrx_chan, 
                   pmt_list3(invocation_handle, s_err_usrp_not_opened, pmt_from_long(0)));
        return;
      }

      d_cs->send(s_response_nrx_chan, 
                 pmt_list3(invocation_handle, 
                           PMT_T, 
                           pmt_from_long(d_nrx_chan)));
      return;
    }	
    //--------- ALLOCATION? -----------//
    else if (pmt_eq(event, s_cmd_current_capacity_allocation)) {
      
      if(!d_opened) { 
        d_cs->send(s_response_current_capacity_allocation, 
                   pmt_list3(invocation_handle, 
                             s_err_usrp_not_opened, 
                             pmt_from_long(0)));
        return;
      }
      
      d_cs->send(s_response_current_capacity_allocation, 
                 pmt_list3(invocation_handle, 
                           PMT_T, 
                           pmt_from_long(current_capacity_allocation())));
      return;
    }
    goto unhandled;
  }
  
  //-------------- TX ---------------//
  if ((port = tx_port_index(port_id)) != -1) {
    
    //------------ ALLOCATE (TX) ----------------//
    if (pmt_eq(event, s_cmd_allocate_channel)){
      
      if(!d_opened) { 
        d_tx[port]->send(s_response_allocate_channel, 
                          pmt_list3(invocation_handle, 
                                    s_err_usrp_not_opened, 
                                    pmt_from_long(0)));
        return;
      }
        
      handle_cmd_allocate_channel(d_tx[port], d_chaninfo_tx, data);
      return;
    }
  
    //----------- DEALLOCATE (TX) ---------------//
    if (pmt_eq(event, s_cmd_deallocate_channel)) {
    
      if(!d_opened) {
        d_tx[port]->send(s_response_deallocate_channel, 
                         pmt_list3(invocation_handle, 
                                   s_err_usrp_not_opened, 
                                   pmt_from_long(0)));
        return;
      }

      handle_cmd_deallocate_channel(d_tx[port], d_chaninfo_tx, data);
      return;
    }
  
    //-------------- XMIT RAW FRAME -----------------/
    if (pmt_eq(event, s_cmd_xmit_raw_frame)){

      if(!d_opened) { 
        d_tx[port]->send(s_response_xmit_raw_frame, 
                         pmt_list2(invocation_handle, s_err_usrp_not_opened));
        return;
      }
      
      handle_cmd_xmit_raw_frame(d_tx[port], d_chaninfo_tx, data);
      return;
    }
    
    //-------------- CONTROL PACKET -----------------/
    if (pmt_eq(event, s_cmd_to_control_channel)) {
      
      if(!d_opened) { 
        d_tx[port]->send(s_response_xmit_raw_frame, 
                         pmt_list2(invocation_handle, s_err_usrp_not_opened));
        return;
      }
      
      handle_cmd_to_control_channel(d_tx[port], d_chaninfo_tx, data);
      return;

    }

    goto unhandled;
  }

  //-------------- RX ---------------//
  if ((port = rx_port_index(port_id)) != -1) {
    
    //------------ ALLOCATE (RX) ----------------//
    if (pmt_eq(event, s_cmd_allocate_channel)) {
      
      if(!d_opened) { 
        d_rx[port]->send(s_response_allocate_channel, 
                          pmt_list3(invocation_handle, 
                                    s_err_usrp_not_opened, 
                                    pmt_from_long(0)));
        return;
      }
        
      handle_cmd_allocate_channel(d_rx[port], d_chaninfo_rx, data);
      return;
    }
  
    //----------- DEALLOCATE (RX) ---------------//
    if (pmt_eq(event, s_cmd_deallocate_channel)) {
    
      if(!d_opened) {
        d_rx[port]->send(s_response_deallocate_channel, 
                         pmt_list3(invocation_handle, 
                                   s_err_usrp_not_opened, 
                                   pmt_from_long(0)));
        return;
      }

      handle_cmd_deallocate_channel(d_rx[port], d_chaninfo_rx, data);
      return;
    }
  
    //-------------- START RECV ----------------//
    if (pmt_eq(event, s_cmd_start_recv_raw_samples)) {
    
      if(!d_opened) {
        d_rx[port]->send(s_response_recv_raw_samples,
                         pmt_list2(invocation_handle, s_err_usrp_not_opened));
        return;
      }

      handle_cmd_start_recv_raw_samples(d_rx[port], d_chaninfo_rx, data);
      return;
    }
    
    //-------------- STOP RECV ----------------//
    if (pmt_eq(event, s_cmd_stop_recv_raw_samples)) {
    
      if(!d_opened) 
        return;

      // FIX ME : no response for stopping? even if error? (permissions)
      handle_cmd_stop_recv_raw_samples(d_rx[port], d_chaninfo_rx, data);

      return;
    }

    goto unhandled;
  }

 unhandled:
  std::cout << "[USRP_SERVER] unhandled msg: " << msg << std::endl;
}

/*!
 * \brief Takes a port_symbol() as parameter \p port_id and is used to determine
 * if the port is a TX port, or to find an index in the d_tx vector which stores
 * the port.
 *
 * \returns -1 if \p port_id is not in the d_tx vector (i.e., it's not a TX
 * port), otherwise returns an index in the d_tx vector which stores the port.
 */
int usrp_server::tx_port_index(pmt_t port_id) {

  for(int i=0; i < (int) d_tx.size(); i++) 
    if(pmt_eq(d_tx[i]->port_symbol(), port_id))
      return i;

  return -1;
}

/*!
 * \brief Takes a port_symbol() as parameter \p port_id and is used to determine
 * if the port is an RX port, or to find an index in the d_rx vector which
 * stores the port.
 *
 * \returns -1 if \p port_id is not in the d_rx vector (i.e., it's not an RX
 * port), otherwise returns an index in the d_rx vector which stores the port.
 */
int usrp_server::rx_port_index(pmt_t port_id) {
  
  for(int i=0; i < (int) d_rx.size(); i++) 
    if(pmt_eq(d_rx[i]->port_symbol(), port_id))
      return i;

  return -1;
}

/*!
 * \brief Determines the current total capacity allocated by all RX and TX
 * channels.
 *
 * \returns the total allocated capacity
 */
long usrp_server::current_capacity_allocation() {
  long capacity = 0;

  for(int chan=0; chan < d_ntx_chan; chan++) 
    capacity += d_chaninfo_tx[chan].assigned_capacity;

  for(int chan=0; chan < d_nrx_chan; chan++)
    capacity += d_chaninfo_rx[chan].assigned_capacity;

  return capacity;
}
    

/*!
 * \brief Called by the handle_message() method if the incoming message to
 * usrp_server is to allocate a channel (cmd-allocate-channel).  The method
 * checks if the requested capacity exists and if so it will reserve it for the
 * caller on the channel that is returned via a response-allocate-channel
 * signal.
 */
void 
usrp_server::handle_cmd_allocate_channel(
                                mb_port_sptr port, 
                                std::vector<struct channel_info> &chan_info,
                                pmt_t data)
{
  pmt_t invocation_handle = pmt_nth(0, data);
  long rqstd_capacity = pmt_to_long(pmt_nth(1, data));
  long chan;

  // Check capacity exists
  if((D_USB_CAPACITY - current_capacity_allocation()) < rqstd_capacity) {

    // no capacity available
    port->send(s_response_allocate_channel, 
               pmt_list3(invocation_handle, 
                         s_err_requested_capacity_unavailable, 
                         PMT_NIL));
    return;
  }

  // Find a free channel, assign the capacity and respond
  for(chan=0; chan < (long)chan_info.size(); chan++) {

    if(verbose)
      std::cout << "[USRP_SERVER] Checking chan: " << chan
                << " owner " << chan_info[chan].owner
                << " size " << chan_info.size()
                << std::endl;

    if(chan_info[chan].owner == PMT_NIL) {
  
      chan_info[chan].owner = port->port_symbol();
      chan_info[chan].assigned_capacity = rqstd_capacity;
      
      port->send(s_response_allocate_channel, 
                 pmt_list3(invocation_handle, 
                           PMT_T, 
                           pmt_from_long(chan)));

      if(verbose)
        std::cout << "[USRP_SERVER] Assigning channel: " << chan 
                  << " to " << chan_info[chan].owner
                  << std::endl;
      return;
    }
  
  }

  if (verbose)
    std::cout << "[USRP_SERVER] Couldnt find a TX chan\n";

  // no free TX chan found
  port->send(s_response_allocate_channel, 
             pmt_list3(invocation_handle, 
                       s_err_channel_unavailable, 
                       PMT_NIL));
  return;
}

/*!
 * \brief Called by the handle_message() method if the incoming message to
 * usrp_server is to deallocate a channel (cmd-deallocate-channel).  The method
 * ensures that the sender of the signal owns the channel and that the channel
 * number is valid.  A response-deallocate-channel signal is sent back with the
 * result of the deallocation.
 */
void 
usrp_server::handle_cmd_deallocate_channel(
                              mb_port_sptr port, 
                              std::vector<struct channel_info> &chan_info, 
                              pmt_t data)
{

  pmt_t invocation_handle = pmt_nth(0, data); 
  long channel = pmt_to_long(pmt_nth(1, data));

  // Ensure the channel is valid and the caller owns the port
  if(!check_valid(port, channel, chan_info,
                  pmt_list2(s_response_deallocate_channel, invocation_handle)))
    return;
  
  chan_info[channel].assigned_capacity = 0;
  chan_info[channel].owner = PMT_NIL;

  port->send(s_response_deallocate_channel, 
             pmt_list2(invocation_handle, 
                       PMT_T));
  return;
}

/*!
 * \brief Called by the handle_message() method if the incoming message to
 * usrp_server is to transmit a frame (cmd-xmit-raw-frame).  The method
 * allocates enough memory to support a burst of packets which contain the frame
 * over the bus of the frame, sets the packet headers, and sends a signal to the
 * lower block for the data (packets) to be written to the bus.  
 *
 * The \p port the command was sent on and the channel info (\p chan_info) of
 * the channel the frame is to be transmitted on are passed to ensure that the
 * caller owns the channel.
 *
 * The \p data parameter is in the format of a cmd-xmit-raw-frame signal.
 *
 * The properties
 */
void usrp_server::handle_cmd_xmit_raw_frame(
                              mb_port_sptr port, 
                              std::vector<struct channel_info> &chan_info, 
                              pmt_t data) 
{
  size_t n_bytes, psize;
  long max_payload_len = transport_pkt::max_payload();

  pmt_t invocation_handle = pmt_nth(0, data);
  long channel = pmt_to_long(pmt_nth(1, data));
  const void *samples = pmt_uniform_vector_elements(pmt_nth(2, data), n_bytes);
  long timestamp = pmt_to_long(pmt_nth(3, data));
  pmt_t properties = pmt_nth(4, data);
  
  // Ensure the channel is valid and the caller owns the port
  if(!check_valid(port, channel, chan_info,
                  pmt_list2(s_response_xmit_raw_frame, invocation_handle)))
    return;

  // Read information from the properties of the packet
  bool carrier_sense = false;
  if(pmt_is_dict(properties)) {

    // Check if carrier sense is enabled for the frame
    if(pmt_t p_carrier_sense = pmt_dict_ref(properties, 
                                            pmt_intern("carrier-sense"), 
                                            PMT_NIL)) {
      if(pmt_eqv(p_carrier_sense, PMT_T)) 
        carrier_sense = true;
    }
  }

  
  // Determine the number of packets to allocate contiguous memory for
  // bursting over the USB and get a pointer to the memory to be used in
  // building the packets
  long n_packets = 
    static_cast<long>(std::ceil(n_bytes / (double)max_payload_len));

  pmt_t v_packets = pmt_make_u8vector(sizeof(transport_pkt) * n_packets, 0);

  transport_pkt *pkts =
    (transport_pkt *) pmt_u8vector_writable_elements(v_packets, psize);

  for(int n=0; n < n_packets; n++) {

    long payload_len = 
      std::min((long)(n_bytes-(n*max_payload_len)), (long)max_payload_len);
  
    if(n == 0) { // first packet gets start of burst flag and timestamp
      
      if(carrier_sense)
        pkts[n].set_header(pkts[n].FL_START_OF_BURST 
                           | pkts[n].FL_CARRIER_SENSE, 
                           channel, 0, payload_len);
      else
        pkts[n].set_header(pkts[n].FL_START_OF_BURST, channel, 0, payload_len);

      pkts[n].set_timestamp(timestamp);
    
    } else {
      pkts[n].set_header(0, channel, 0, payload_len);
      pkts[n].set_timestamp(0xffffffff);
    }

    memcpy(pkts[n].payload(), 
           (uint8_t *)samples+(max_payload_len * n), 
           payload_len);
  
  }


  pkts[n_packets-1].set_end_of_burst(); // set the last packet's end of burst

  if (verbose && 0)
    std::cout << "[USRP_SERVER] Received raw frame invocation: " 
              << invocation_handle << std::endl;
    
  // The actual response to the write will be generated by a
  // s_response_usrp_write since we cannot determine whether to transmit was
  // successful until we hear from the lower layers.
  d_cs_usrp->send(s_cmd_usrp_write, 
                  pmt_list3(invocation_handle, 
                            pmt_from_long(channel), 
                            v_packets));

  return;
}

/*!
 * \brief Called by the handle_message() method to parse incoming control/status
 * signals (cmd-to-control-channel).  
 * 
 * The \p port the command was sent on and the channel info (\p chan_info) of
 * the channel are passed to ensure that the caller owns the channel.
 *
 * The \p data parameter is in the format of a PMT list, where each element
 * follows the format of a control/status signal (i.e. op-ping-fixed).
 *
 * The method will parse all of the C/S commands included in \p data and place
 * the commands in to a lower level packet sent to the control channel.  The
 * method will pack as many commands as possible in t oa single packet, and once
 * it is fill generate as many lower level packets as needed.
 *
 * Anything that needs to be returned to the sender of the signal (i.e. the
 * value of a register) will be generated by the parse_control_pkt() method as
 * the responses to the commands are read back from the USRP.
 */
void usrp_server::handle_cmd_to_control_channel(
                            mb_port_sptr port, 
                            std::vector<struct channel_info> &chan_info, 
                            pmt_t data) 
{

  pmt_t invocation_handle = pmt_nth(0, data);
  pmt_t subpackets = pmt_nth(1, data);

  long n_subpkts = pmt_length(subpackets);
  long curr_subpkt = 0;

  size_t psize;
  long payload_len = 0;
  long channel = CONTROL_CHAN;

  if(verbose)
    std::cout << "[USRP_SERVER] Handling " << n_subpkts << " commands\n";

  // The design of the following code is optimized for simplicity, not
  // performance.  To performance optimize this code, the total size in bytes
  // needed for all of the CS packets is needed to allocate contiguous memory
  // which contains the USB packets for bursting over the bus.  However to do
  // this the packets subpackets would need to be parsed twice and their sizes
  // would need to be determined.
  //
  // The approach taken is to keep parsing the subpackets and putting them in to
  // USB packets.  Once the USB packet is full, a write is sent for it and
  // another packet is created.
  //
  // The subpacket creation methods will return false if the subpacket will not
  // fit in to the current USB packet.  In these cases a new USB packet is
  // created and the old is sent.
  
  new_packet:
    // This code needs to become "smart" and only make a new packet when full
    pmt_t v_packet = pmt_make_u8vector(sizeof(transport_pkt), 0);
    transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_packet, psize);
    payload_len = 0;
    
    pkt->set_header(0, channel, 0, payload_len);
    pkt->set_timestamp(0xffffffff);

  while(curr_subpkt < n_subpkts) {

    pmt_t subp = pmt_nth(curr_subpkt, subpackets);
    pmt_t subp_cmd = pmt_nth(0, subp);
    pmt_t subp_data = pmt_nth(1, subp);

    //--------- PING FIXED --------------//
    if(pmt_eq(subp_cmd, s_op_ping_fixed)) {

      long urid     = pmt_to_long(pmt_nth(0, subp_data));
      long pingval  = pmt_to_long(pmt_nth(1, subp_data));

      // USRP server sets request ID's to keep track of which application gets
      // what response back.  To allow a full 6-bits for an RID to the user, we
      // keep a mapping and replace the RID's as the packets go in and out.  If
      // there are no RID's available, the command is thrown away silently. 
      long srid;
      if((srid = next_rid()) == -1)
        goto subpkt_bail;

      // We use a vector to store the owner of the ping request and will use it
      // to send the request on any RX port they own. 
      d_rids[srid].owner = port->port_symbol();
      d_rids[srid].user_rid = urid;
        
      // Adds a ping after the previous command in the pkt
      if(!pkt->cs_ping(srid, pingval))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));

        // Return the RID
        d_rids[srid].owner = PMT_NIL;

        goto new_packet;
      }

      if(verbose)
        std::cout << "[USRP_SERVER] Received ping command request"
                  << " assigning RID " << srid << std::endl;

    }
  
    //----------- WRITE REG ---------------//
    if(pmt_eq(subp_cmd, s_op_write_reg)) {
      
      long reg_num = pmt_to_long(pmt_nth(0, subp_data));
      long val = pmt_to_long(pmt_nth(1, subp_data));

      if(!pkt->cs_write_reg(reg_num, val))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));
        
        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received write register request "
                  << "("
                  << "Reg: " << reg_num << ", "
                  << "Val: " << val
                  << ")\n";
    }
    
    //------- WRITE REG MASKED ----------//
    if(pmt_eq(subp_cmd, s_op_write_reg_masked)) {
      
      long reg_num = pmt_to_long(pmt_nth(0, subp_data));
      long val = pmt_to_long(pmt_nth(1, subp_data));
      long mask = pmt_to_long(pmt_nth(2, subp_data));

      if(!pkt->cs_write_reg_masked(reg_num, val, mask))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));
        
        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received write register masked request\n";
    }
    
    //------------ READ REG --------------//
    if(pmt_eq(subp_cmd, s_op_read_reg)) {
      
      long urid     = pmt_to_long(pmt_nth(0, subp_data));
      long reg_num  = pmt_to_long(pmt_nth(1, subp_data));

      long srid;
      if((srid = next_rid()) == -1)
        goto subpkt_bail;

      d_rids[srid].owner = port->port_symbol();
      d_rids[srid].user_rid = urid;

      if(!pkt->cs_read_reg(srid, reg_num))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));

        // Return the rid
        d_rids[srid].owner = PMT_NIL;
        
        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received read register request"
                  << " assigning RID " << srid << std::endl;
    }
    
    //------------ DELAY --------------//
    if(pmt_eq(subp_cmd, s_op_delay)) {

      long ticks = pmt_to_long(pmt_nth(0, subp_data));

      if(!pkt->cs_delay(ticks))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));
        
        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received delay request of "
                  << ticks << " ticks\n";
    }

    //--------- I2C WRITE -----------//
    // FIXME: could check that byte count does not exceed 2^8 which
    // is the max length in the subpacket for # of bytes to read.
    if(pmt_eq(subp_cmd, s_op_i2c_write)) {
      
      long i2c_addr = pmt_to_long(pmt_nth(0, subp_data));
      pmt_t data = pmt_nth(1, subp_data);

      // Get a readable address to the data which also gives us the length
      size_t data_len;
      uint8_t *i2c_data = (uint8_t *) pmt_u8vector_writable_elements(data, data_len);

      // Make the USB packet
      if(!pkt->cs_i2c_write(i2c_addr, i2c_data, data_len))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));
        
        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received I2C write\n";
    }
  
    //----------- I2C Read -------------//
    if(pmt_eq(subp_cmd, s_op_i2c_read)) {
      
      long urid       = pmt_to_long(pmt_nth(0, subp_data));
      long i2c_addr   = pmt_to_long(pmt_nth(1, subp_data));
      long i2c_bytes  = pmt_to_long(pmt_nth(2, subp_data));

      long srid;
      if((srid = next_rid()) == -1)
        goto subpkt_bail;
      
      d_rids[srid].owner = port->port_symbol();
      d_rids[srid].user_rid = urid;

      if(!pkt->cs_i2c_read(srid, i2c_addr, i2c_bytes))
      {
        
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));

        d_rids[srid].owner = PMT_NIL;

        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received I2C read\n";
    }
    
    //--------- SPI WRITE -----------//
    if(pmt_eq(subp_cmd, s_op_spi_write)) {
      
      long enables = pmt_to_long(pmt_nth(0, subp_data));
      long format = pmt_to_long(pmt_nth(1, subp_data));
      long opt = pmt_to_long(pmt_nth(2, subp_data));
      pmt_t data = pmt_nth(3, subp_data);

      // Get a readable address to the data which also gives us the length
      size_t data_len;
      uint8_t *spi_data = (uint8_t *) pmt_u8vector_writable_elements(data, data_len);

      // Make the USB packet
      if(!pkt->cs_spi_write(enables, format, opt, spi_data, data_len))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));
        
        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received SPI write\n";
    }
    
    //--------- SPI READ -----------//
    if(pmt_eq(subp_cmd, s_op_spi_read)) {
      
      long urid     = pmt_to_long(pmt_nth(0, subp_data));
      long enables  = pmt_to_long(pmt_nth(1, subp_data));
      long format   = pmt_to_long(pmt_nth(2, subp_data));
      long opt      = pmt_to_long(pmt_nth(3, subp_data));
      long n_bytes  = pmt_to_long(pmt_nth(4, subp_data));
      
      long srid;
      if((srid = next_rid()) == -1)
        goto subpkt_bail;

      d_rids[srid].owner = port->port_symbol();
      d_rids[srid].user_rid = urid;

      // Make the USB packet
      if(!pkt->cs_spi_read(srid, enables, format, opt, n_bytes))
      {
        d_cs_usrp->send(s_cmd_usrp_write, 
                        pmt_list3(invocation_handle, 
                                  pmt_from_long(channel), 
                                  v_packet));
        
        // Return the rid
        d_rids[srid].owner = PMT_NIL;

        goto new_packet;
      }
      
      if(verbose)
        std::cout << "[USRP_SERVER] Received SPI read\n";
    }

  subpkt_bail:
    curr_subpkt++;

  }


  // If the current packets length is > 0, we know there are subpackets that
  // need to be sent out still.
  if(pkt->payload_len() > 0)
    d_cs_usrp->send(s_cmd_usrp_write, 
                    pmt_list3(invocation_handle, 
                              pmt_from_long(channel), 
                              v_packet));

  return;
}

/*!
 * \brief Called by the handle_message() method when the incoming signal is a
 * command to start reading samples from the USRP (cmd-start-recv-raw-samples).  
 *
 * The \p port the command was sent on and the channel info (\p chan_info) of
 * the channel are passed to ensure that the caller owns the channel.
 *
 * The \p data parameter should be in the format of a cmd-start-recv-raw-samples
 * command where the first element in the list is an invocation handle, and the
 * second is the channel the signal generator wants to receive the samples on.
 */
void
usrp_server::handle_cmd_start_recv_raw_samples(
                                  mb_port_sptr port, 
                                  std::vector<struct channel_info> &chan_info, 
                                  pmt_t data)
{
  pmt_t invocation_handle = pmt_nth(0, data);
  long channel = pmt_to_long(pmt_nth(1, data));

  // Ensure the channel is valid and the caller owns the port
  if(!check_valid(port, channel, chan_info,
                  pmt_list2(s_response_xmit_raw_frame, invocation_handle)))
    return;

  // Already started receiving samples? (another start before a stop)
  // Check the RX channel bitmask.
  if(d_rx_chan_mask & (1 << channel)) {
    port->send(s_response_recv_raw_samples,
               pmt_list5(invocation_handle,
                         s_err_already_receiving,
                         PMT_NIL,
                         PMT_NIL,
                         PMT_NIL));
    return;
  }

  // We only need to generate a 'start reading' command down to the
  // low level interface if no other channel is already reading
  //
  // We carry this over the CS interface because the lower level
  // interface does not care about the channel, we only demux it
  // at the usrp_server on responses.
  if(d_rx_chan_mask == 0) {
    
    if(verbose)
      std::cout << "[USRP_SERVER] Sending read request down to start recv\n";

    d_cs_usrp->send(s_cmd_usrp_start_reading, pmt_list1(invocation_handle));
  }

  d_rx_chan_mask |= 1<<channel;
  
  return;
}

/*!
 * \brief Called by the handle_message() method when the incoming signal is to
 * stop receiving samples from the USRP (cmd-stop-recv-raw-samples).
 *
 * The \p port the command was sent on and the channel info (\p chan_info) of
 * the channel are passed to ensure that the caller owns the channel.
 *
 * The \p data parameter should be in the format of a cmd-stop-recv-raw-samples
 * command where the first element in the list is an invocation handle, and the
 * second is the channel the signal generator wants to stop receiving the
 * samples from.
 */
void
usrp_server::handle_cmd_stop_recv_raw_samples(
                        mb_port_sptr port, 
                        std::vector<struct channel_info> &chan_info, 
                        pmt_t data)
{
  pmt_t invocation_handle = pmt_nth(0, data);
  long channel = pmt_to_long(pmt_nth(1, data));

  // FIX ME : we have no responses to send an error...
  // Ensure the channel is valid and the caller owns the port
  //if(!check_valid(port, channel, chan_info,
  //                pmt_list2(s_response_xmit_raw_frame, invocation_handle)))
  //  return;

  // Remove this hosts bit from the receiver mask
  d_rx_chan_mask &= ~(1<<channel);

  // We only need to generate a 'start reading' command down to the
  // low level interface if no other channel is already reading
  //
  // We carry this over the CS interface because the lower level
  // interface does not care about the channel, we only demux it
  // at the usrp_server on responses.
  if(d_rx_chan_mask == 0) {
    
    if(verbose)
      std::cout << "[USRP_SERVER] Sending stop reading request down\n";

    d_cs_usrp->send(s_cmd_usrp_stop_reading, pmt_list1(invocation_handle));
  }
  
  return;
}

/*!
 * \brief Called by the handle_message() method when an incoming signal is
 * generated to USRP server that contains raw samples from the USRP.  This
 * method generates the response-recv-raw-samples signals that are the result of
 * a cmd-start-recv-raw-samples signal.
 *
 * The raw lower-level packet is extracted from \p data, where the format for \p
 * data is a PMT list.  The PMT \p data list should contain an invocation handle
 * as the first element, the status of the lower-level read as the second
 * element, and a uniform vector representation of the packets as the third
 * element.  
 *
 * The packet contains a channel field that the samples are destined to, and the
 * method determines where to send the samples based on this channel since each
 * channel has an associated port which allocated it.
 */
void
usrp_server::handle_response_usrp_read(pmt_t data)
{

  pmt_t invocation_handle = pmt_nth(0, data);
  pmt_t status = pmt_nth(1, data);
  pmt_t v_pkt = pmt_nth(2, data);

  size_t n_bytes;
  size_t ignore;

  if (d_fake_rx) {

    pmt_t pkt = pmt_nth(2, data);

    d_rx[0]->send(s_response_recv_raw_samples,
                  pmt_list5(PMT_F,
                            PMT_T,
                            pkt,
                            pmt_from_long(0xffff),
                            PMT_NIL));

    return;
  }

  // Extract the packet and return appropriately
  transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_pkt, n_bytes);

  // The channel is used to find the port to pass the samples on
  long channel = pkt->chan();
  long payload_len = pkt->payload_len();
  long port;

  // Ignore packets which seem to have incorrect size or size 0
  if(payload_len > pkt->max_payload() || payload_len == 0)
    return;
  
  // If the packet is a C/S packet, parse it separately
  if(channel == CONTROL_CHAN) {
    parse_control_pkt(invocation_handle, pkt);
    return;
  }

  if((port = rx_port_index(d_chaninfo_rx[channel].owner)) == -1)
    return; // Don't know where to send the sample... possibility on abrupt close
    
  pmt_t v_samples = pmt_make_u8vector(payload_len, 0);
  uint8_t *samples = pmt_u8vector_writable_elements(v_samples, ignore);
  
  memcpy(samples, pkt->payload(), payload_len);

  // Build a properties dictionary to store things such as the RSSI
  pmt_t properties =  pmt_make_dict();

  pmt_dict_set(properties,
               pmt_intern("rssi"),
               pmt_from_long(pkt->rssi()));

  if(pkt->overrun())
    pmt_dict_set(properties,
                 pmt_intern("overrun"),
                 PMT_T);

  if(pkt->underrun())
    pmt_dict_set(properties,
                 pmt_intern("underrun"),
                 PMT_T);

  d_rx[port]->send(s_response_recv_raw_samples,
                   pmt_list6(invocation_handle,
                             status,
                             v_samples,
                             pmt_from_long(pkt->timestamp()),
                             pmt_from_long(channel),
                             properties));
  return;
}

/*!
 * \brief Called by handle_response_usrp_read() when the incoming packet has a
 * channel of CONTROL_CHAN.  This means that the incoming packet contains a
 * response for a command sent to the control channel, which this method will
 * parse.
 *
 * The \p pkt parameter is a pointer to the full packet (transport_pkt) in
 * memory.
 *
 * Given that all commands sent to the control channel that require responses
 * will carry an RID (request ID), the method will use the RID passed back with
 * the response to determine which port the response should be sent on.
 */
void
usrp_server::parse_control_pkt(pmt_t invocation_handle, transport_pkt *pkt)
{

  long payload_len = pkt->payload_len();
  long curr_payload = 0;
  long port;
  
  // We dispatch based on the control packet type, however we can extract the
  // opcode and the length immediately which is consistent in all responses.
  //
  // Since each control packet can have multiple responses, we keep reading the
  // lengths of each subpacket until we reach the payload length.  
  while(curr_payload < payload_len) {

    pmt_t sub_packet = pkt->read_subpacket(curr_payload);
    pmt_t op_symbol = pmt_nth(0, sub_packet);

    int len = pkt->cs_len(curr_payload);

    if(verbose)
      std::cout << "[USRP_SERVER] Parsing subpacket " 
                << op_symbol << " ... length " << len << std::endl;

    //----------------- PING RESPONSE ------------------//
    if(pmt_eq(op_symbol, s_op_ping_fixed_reply)) {

      long srid     = pmt_to_long(pmt_nth(1, sub_packet));
      pmt_t pingval = pmt_nth(2, sub_packet);

      long urid = d_rids[srid].user_rid;
      
      if(verbose)
        std::cout << "[USRP_SERVER] Found ping response "
                  << "("
                  << "URID: " << urid << ", "
                  << "SRID: " << srid << ", "
                  << "VAL: " << pingval 
                  << ")\n";
      
      // Do some bounds checking incase of bogus/corrupt responses
      if(srid > D_MAX_RID)
        return;

      pmt_t owner = d_rids[srid].owner;
      
      // Return the RID
      d_rids[srid].owner = PMT_NIL;

      // FIXME: should be 1 response for all subpackets here ?
      if((port = tx_port_index(owner)) != -1)
        d_tx[port]->send(s_response_from_control_channel,
                         pmt_list4(invocation_handle,
                                   PMT_T,
                                   pmt_list2(s_op_ping_fixed_reply, // subp
                                             pmt_list2(pmt_from_long(urid), 
                                                       pingval)),
                                   pmt_from_long(pkt->timestamp())));
    }
    
    //----------------- READ REG RESPONSE ------------------//
    else if(pmt_eq(op_symbol, s_op_read_reg_reply)) {

      long srid     = pmt_to_long(pmt_nth(1, sub_packet));
      pmt_t reg_num = pmt_nth(2, sub_packet);
      pmt_t reg_val = pmt_nth(3, sub_packet);

      long urid = d_rids[srid].user_rid;
      
      if(verbose)
        std::cout << "[USRP_SERVER] Found read register response "
                  << "("
                  << "URID: " << urid << ", "
                  << "SRID: " << srid << ", "
                  << "REG: " << reg_num << ", "
                  << "VAL: " << reg_val 
                  << ")\n";

      // Do some bounds checking to avoid seg faults
      if(srid > D_MAX_RID)
        return;
      
      pmt_t owner = d_rids[srid].owner;
      
      // Return the RID
      d_rids[srid].owner = PMT_NIL;

      // FIXME: should be 1 response for all subpackets here ?
      if((port = tx_port_index(owner)) != -1)
        d_tx[port]->send(s_response_from_control_channel,
                         pmt_list4(invocation_handle,
                                   PMT_T,
                                   pmt_list2(s_op_read_reg_reply, // subp
                                             pmt_list3(pmt_from_long(urid), 
                                                       reg_num, 
                                                       reg_val)),
                                   pmt_from_long(pkt->timestamp())));
    }

    //------------------ I2C READ REPLY -------------------//
    else if(pmt_eq(op_symbol, s_op_i2c_read_reply)) {

      long srid       = pmt_to_long(pmt_nth(1, sub_packet));
      pmt_t i2c_addr  = pmt_nth(2, sub_packet);
      pmt_t i2c_data  = pmt_nth(3, sub_packet);

      long urid = d_rids[srid].user_rid;

      if(verbose)
        std::cout << "[USRP_SERVER] Found i2c read reply "
                  << "("
                  << "URID: " << urid << ", "
                  << "SRID: " << srid << ", "
                  << "Addr: " << i2c_addr << ", "
                  << "Data: " << i2c_data
                  << ")\n";
      
      // Do some bounds checking to avoid seg faults
      if(srid > D_MAX_RID)
        return;

      pmt_t owner = d_rids[srid].owner;
      
      // Return the RID
      d_rids[srid].owner = PMT_NIL;

      if((port = tx_port_index(owner)) != -1)
        d_tx[port]->send(s_response_from_control_channel,
                         pmt_list4(invocation_handle,
                                   PMT_T,
                                   pmt_list2(s_op_i2c_read_reply,
                                             pmt_list3(pmt_from_long(urid), 
                                                       i2c_addr,
                                                       i2c_data)),
                                   pmt_from_long(pkt->timestamp())));
    }

    //------------------ SPI READ REPLY -------------------//
    else if(pmt_eq(op_symbol, s_op_spi_read_reply)) {
      
      long srid       = pmt_to_long(pmt_nth(1, sub_packet));
      pmt_t spi_data  = pmt_nth(2, sub_packet);
      
      long urid = d_rids[srid].user_rid;

      if(verbose)
        std::cout << "[USRP_SERVER] Found SPI read reply "
                  << "("
                  << "URID: " << urid << ", "
                  << "SRID: " << srid << ", "
                  << "Data: " << spi_data
                  << ")\n";

      // Bounds check the RID
      if(srid > D_MAX_RID)
        return;

      pmt_t owner = d_rids[srid].owner;
      
      // Return the RID
      d_rids[srid].owner = PMT_NIL;

      if((port = tx_port_index(owner)) != -1)
        d_tx[port]->send(s_response_from_control_channel,
                         pmt_list4(invocation_handle,
                                   PMT_T,
                                   pmt_list2(s_op_spi_read_reply,
                                             pmt_list2(pmt_from_long(urid), 
                                                       spi_data)),
                                   pmt_from_long(pkt->timestamp())));
    }

    // Each subpacket has an unaccounted for 2 bytes which is the opcode
    // and the length field
    curr_payload += len + 2;
    
    // All subpackets are 32-bit aligned
    int align_offset = 4 - (curr_payload % 4);

    if(align_offset != 4)
      curr_payload += align_offset;
  }
}

/*!
 * \brief Used to recall all incoming signals that were deferred when USRP
 * server was in the initialization state.
 */
void
usrp_server::recall_defer_queue()
{

  std::vector<mb_message_sptr> recall;

  while(!d_defer_queue.empty()) {
    recall.push_back(d_defer_queue.front());
    d_defer_queue.pop();
  }

  // Parse the messages that were queued while waiting for an open response
  for(int i=0; i < (int)recall.size(); i++) 
    handle_message(recall[i]);

  return;
}

/*!
 * \brief Commonly called by any method which handles outgoing frames or control
 * packets to the USRP to check if the port on which the signal was sent owns
 * the channel the outgoing packet will be associated with.   This helps ensure
 * that applications do not send data on other application's ports.
 *
 * The \p port parameter is the port symbol that the caller wishes to determine
 * owns the channel specified by \p chan_info.  
 *
 * The \p signal_info parameter is a PMT list containing two elements: the
 * response signal to use if the permissions are invalid, and the invocation
 * handle that was passed.  This allows the method to generate detailed failure
 * responses to signals without having to return some sort of structured
 * information which the caller must then parse and interpret to determine the
 * failure type.
 *
 * \returns true if \p port owns the channel specified by \p chan_info, false
 * otherwise.
 */
bool
usrp_server::check_valid(mb_port_sptr port,
                         long channel,
                         std::vector<struct channel_info> &chan_info,
                         pmt_t signal_info)
{

  pmt_t response_signal = pmt_nth(0, signal_info);
  pmt_t invocation_handle = pmt_nth(1, signal_info);

  // not a valid channel number?
  if(channel >= (long)chan_info.size() && channel != CONTROL_CHAN) {
    port->send(response_signal, 
               pmt_list2(invocation_handle, 
                         s_err_channel_invalid));

    if(verbose)
      std::cout << "[USRP_SERVER] Invalid channel number for event " 
                << response_signal << std::endl;
    return false;
  }
  
  // not the owner of the port?
  if(chan_info[channel].owner != port->port_symbol()) {
    port->send(response_signal, 
               pmt_list2(invocation_handle, 
                         s_err_channel_permission_denied));
    
    if(verbose)
      std::cout << "[USRP_SERVER] Invalid permissions"
                << " for " << response_signal
                << " from " << port->port_symbol()
                << " proper owner is " << chan_info[channel].owner
                << " on channel " << channel
                << " invocation " << invocation_handle
                << std::endl;
    return false;
  }

  return true;
}

/*!
 * \brief Finds the next available RID for internal USRP server use with control
 * and status packets.
 *
 * \returns the next valid RID or -1 if no more RIDs are available.
 */
long
usrp_server::next_rid()
{
  for(int i = 0; i < D_MAX_RID; i++)
    if(pmt_eqv(d_rids[i].owner, PMT_NIL))
      return i;

  if(verbose)
    std::cout << "[USRP_SERVER] No RIDs left\n";
  return -1;
}

/*!
 * \brief Called by handle_message() when USRP server gets a response that the
 * USRP was opened successfully to initialize the registers using the new
 * register read/write control packets.
 */
void
usrp_server::initialize_registers()
{
  // We use handle_cmd_to_control_channel() to create the register writes using
  // PMT_NIL as the response port to tell usrp_server not to pass the response
  // up to any application.
  if(verbose)
    std::cout << "[USRP_SERVER] Initializing registers...\n";

  // RX mode to normal (0)
  set_register(FR_MODE, 0);

  // FPGA debugging?
  if(d_fpga_debug) {
    set_register(FR_DEBUG_EN, 1);
    // FIXME: need to figure out exact register writes to control daughterboard
    // pins that need to be written to
  } else {
    set_register(FR_DEBUG_EN, 0);
  }

  // Set the transmit sample rate divisor, which is 4-1
  set_register(FR_TX_SAMPLE_RATE_DIV, 3);

  // Dboard IO buffer and register settings
  set_register(FR_OE_0, (0xffff << 16) | 0x0000);
  set_register(FR_IO_0, (0xffff << 16) | 0x0000);
  set_register(FR_OE_1, (0xffff << 16) | 0x0000);
  set_register(FR_IO_1, (0xffff << 16) | 0x0000);
  set_register(FR_OE_2, (0xffff << 16) | 0x0000);
  set_register(FR_IO_2, (0xffff << 16) | 0x0000);
  set_register(FR_OE_3, (0xffff << 16) | 0x0000);
  set_register(FR_IO_3, (0xffff << 16) | 0x0000);

  // zero Tx side Auto Transmit/Receive regs
  set_register(FR_ATR_MASK_0, 0); 
  set_register(FR_ATR_TXVAL_0, 0);
  set_register(FR_ATR_RXVAL_0, 0);
  set_register(FR_ATR_MASK_1, 0); 
  set_register(FR_ATR_TXVAL_1, 0);
  set_register(FR_ATR_RXVAL_1, 0);
  set_register(FR_ATR_MASK_2, 0);
  set_register(FR_ATR_TXVAL_2, 0);
  set_register(FR_ATR_RXVAL_2, 0);
  set_register(FR_ATR_MASK_3, 0);
  set_register(FR_ATR_TXVAL_3, 0);
  set_register(FR_ATR_RXVAL_3, 0);

  // Configure TX mux, this is a hacked value
  set_register(FR_TX_MUX, 0x00000081);

  // Set the interpolation rate, which is the rate divided by 4, minus 1
  set_register(FR_INTERP_RATE, (d_interp_tx/4)-1);

  // Apparently this register changes again
  set_register(FR_TX_MUX, 0x00000981);

  // Set the receive sample rate divisor, which is 2-1
  set_register(FR_RX_SAMPLE_RATE_DIV, 1);

  // DC offset
  set_register(FR_DC_OFFSET_CL_EN, 0x0000000f);

  // Reset the DC correction offsets
  set_register(FR_ADC_OFFSET_0, 0);
  set_register(FR_ADC_OFFSET_1, 0);

  // Some hard-coded RX configuration
  set_register(FR_RX_FORMAT, 0x00000300);
  set_register(FR_RX_MUX, 1);

  // RX decimation rate is divided by two, then subtract 1
  set_register(FR_DECIM_RATE, (d_decim_rx/2)-1);

  // More hard coding
  set_register(FR_RX_MUX, 0x000e4e41);

  // Resetting RX registers
  set_register(FR_RX_PHASE_0, 0);
  set_register(FR_RX_PHASE_1, 0);
  set_register(FR_RX_PHASE_2, 0);
  set_register(FR_RX_PHASE_3, 0);
  set_register(FR_RX_FREQ_0, 0x28000000);
  set_register(FR_RX_FREQ_1, 0);
  set_register(FR_RX_FREQ_2, 0);
  set_register(FR_RX_FREQ_3, 0);

  // Enable debug bus
  set_register(FR_DEBUG_EN, 0xf);
  set_register(FR_OE_0, -1);
  set_register(FR_OE_1, -1);
  set_register(FR_OE_2, -1);
  set_register(FR_OE_3, -1);

  // DEBUGGING
  //check_register_initialization();
}

// FIXME: used for debugging to determine if all the registers are actually
// being set correctly
void
usrp_server::check_register_initialization()
{
  // RX mode to normal (0)
  read_register(FR_MODE);

  // FPGA debugging?
  if(d_fpga_debug) {
    read_register(FR_DEBUG_EN);
    // FIXME: need to figure out exact register writes to control daughterboard
    // pins that need to be written to
  } else {
    read_register(FR_DEBUG_EN);
  }

  // Set the transmit sample rate divisor, which is 4-1
  read_register(FR_TX_SAMPLE_RATE_DIV);

  // Dboard IO buffer and register settings
  read_register(FR_OE_0);
  read_register(FR_IO_0);
  read_register(FR_OE_1);
  read_register(FR_IO_1);
  read_register(FR_OE_2);
  read_register(FR_IO_2);
  read_register(FR_OE_3);
  read_register(FR_IO_3);

  // zero Tx side Auto Transmit/Receive regs
  read_register(FR_ATR_MASK_0); 
  read_register(FR_ATR_TXVAL_0);
  read_register(FR_ATR_RXVAL_0);
  read_register(FR_ATR_MASK_1); 
  read_register(FR_ATR_TXVAL_1);
  read_register(FR_ATR_RXVAL_1);
  read_register(FR_ATR_MASK_2);
  read_register(FR_ATR_TXVAL_2);
  read_register(FR_ATR_RXVAL_2);
  read_register(FR_ATR_MASK_3);
  read_register(FR_ATR_TXVAL_3);
  read_register(FR_ATR_RXVAL_3);

  // Configure TX mux, this is a hacked value
  read_register(FR_TX_MUX);

  // Set the interpolation rate, which is the rate divided by 4, minus 1
  read_register(FR_INTERP_RATE);

  // Apparently this register changes again
  read_register(FR_TX_MUX);

  // Set the receive sample rate divisor, which is 2-1
  read_register(FR_RX_SAMPLE_RATE_DIV);

  // DC offset
  read_register(FR_DC_OFFSET_CL_EN);

  // Reset the DC correction offsets
  read_register(FR_ADC_OFFSET_0);
  read_register(FR_ADC_OFFSET_1);

  // Some hard-coded RX configuration
  read_register(FR_RX_FORMAT);
  read_register(FR_RX_MUX);

  // RX decimation rate is divided by two, then subtract 1
  read_register(FR_DECIM_RATE);

  // More hard coding
  read_register(FR_RX_MUX);

  // Resetting RX registers
  read_register(FR_RX_PHASE_0);
  read_register(FR_RX_PHASE_1);
  read_register(FR_RX_PHASE_2);
  read_register(FR_RX_PHASE_3);
  read_register(FR_RX_FREQ_0);
  read_register(FR_RX_FREQ_1);
  read_register(FR_RX_FREQ_2);
  read_register(FR_RX_FREQ_3);
}

/*!
 * \brief Used to generate FPGA register write commands to reset all of the FPGA
 * registers to a value of 0.
 */
void
usrp_server::reset_all_registers()
{
  for(int i=0; i<64; i++)
    set_register(i, 0);
}

/*!
 * \brief Used internally by USRP server to generate a control/status packet
 * which contains a register write.
 *
 * The \p reg parameter is the register number that the value \p val will be
 * written to.
 */
void
usrp_server::set_register(long reg, long val)
{
  size_t psize;
  long payload_len = 0;

  pmt_t v_packet = pmt_make_u8vector(sizeof(transport_pkt), 0);
  transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_packet, psize);
  
  pkt->set_header(0, CONTROL_CHAN, 0, payload_len);
  pkt->set_timestamp(0xffffffff);

  pkt->cs_write_reg(reg, val);

  d_cs_usrp->send(s_cmd_usrp_write, 
                  pmt_list3(PMT_NIL, 
                            pmt_from_long(CONTROL_CHAN), 
                            v_packet));
}

/*!
 * \brief Used internally by USRP server to generate a control/status packet
 * which contains a register read.  This is important to use internally so that
 * USRP server can bypass the use of RIDs with register reads, as they are not
 * needed and it would use up the finite number of RIDs available for use for
 * applications to receive responses.
 *
 * The \p reg parameter is the register number that the value should be read
 * from.
 */
void
usrp_server::read_register(long reg)
{
  size_t psize;
  long payload_len = 0;

  pmt_t v_packet = pmt_make_u8vector(sizeof(transport_pkt), 0);
  transport_pkt *pkt = (transport_pkt *) pmt_u8vector_writable_elements(v_packet, psize);
  
  pkt->set_header(0, CONTROL_CHAN, 0, payload_len);
  pkt->set_timestamp(0xffffffff);

  pkt->cs_read_reg(0, reg);

  d_cs_usrp->send(s_cmd_usrp_write, 
                  pmt_list3(PMT_NIL, 
                            pmt_from_long(CONTROL_CHAN), 
                            v_packet));
}

REGISTER_MBLOCK_CLASS(usrp_server);