diff options
Diffstat (limited to 'usrp/host/lib/inband/usrp_server.cc')
| -rw-r--r-- | usrp/host/lib/inband/usrp_server.cc | 1342 |
1 files changed, 1156 insertions, 186 deletions
diff --git a/usrp/host/lib/inband/usrp_server.cc b/usrp/host/lib/inband/usrp_server.cc index 800bf0e15..760397dc7 100644 --- a/usrp/host/lib/inband/usrp_server.cc +++ b/usrp/host/lib/inband/usrp_server.cc @@ -27,39 +27,19 @@ #include <usrp_inband_usb_packet.h> #include <mb_class_registry.h> #include <vector> +#include <usrp_usb_interface.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 -// FIXME We should machine generate these by a simple preprocessor run over this file -// -// These are all the messages that we expect to receive. -// -// We "intern" these here (make them into symbols) so that our -// comparisions below are effectively pointer comparisons. - -static pmt_t s_cmd_allocate_channel = pmt_intern("cmd-allocate-channel"); -static pmt_t s_cmd_close = pmt_intern("cmd-close"); -static pmt_t s_cmd_deallocate_channel = pmt_intern("cmd-deallocate-channel"); -static pmt_t s_cmd_open = pmt_intern("cmd-open"); -static pmt_t s_cmd_start_recv_raw_samples = pmt_intern("cmd-start-recv-raw-samples"); -static pmt_t s_cmd_stop_recv_raw_samples = pmt_intern("cmd-stop-recv-raw-samples"); -static pmt_t s_cmd_to_control_channel = pmt_intern("cmd-to-control-channel"); -static pmt_t s_cmd_xmit_raw_frame = pmt_intern("cmd-xmit-raw-frame"); -static pmt_t s_cmd_max_capacity = pmt_intern("cmd-max-capacity"); -static pmt_t s_cmd_ntx_chan = pmt_intern("cmd-ntx-chan"); -static pmt_t s_cmd_nrx_chan = pmt_intern("cmd-nrx-chan"); -static pmt_t s_cmd_current_capacity_allocation = pmt_intern("cmd-current-capacity-allocation"); -static pmt_t s_response_allocate_channel = pmt_intern("response-allocate-channel"); -static pmt_t s_response_close = pmt_intern("response-close"); -static pmt_t s_response_deallocate_channel = pmt_intern("response-deallocate-channel"); -static pmt_t s_response_from_control_channel = pmt_intern("response-from-control-channel"); -static pmt_t s_response_open = pmt_intern("response-open"); -static pmt_t s_response_recv_raw_samples = pmt_intern("response-recv-raw-samples"); -static pmt_t s_response_xmit_raw_frame = pmt_intern("response-xmit-raw-frame"); -static pmt_t s_response_max_capacity = pmt_intern("response-max-capacity"); -static pmt_t s_response_ntx_chan = pmt_intern("response-ntx-chan"); -static pmt_t s_response_nrx_chan = pmt_intern("response-nrx-chan"); -static pmt_t s_response_current_capacity_allocation = pmt_intern("response-current-capacity-allocation"); +const static bool verbose = false; static std::string str(long x) @@ -70,36 +50,75 @@ str(long x) } usrp_server::usrp_server(mb_runtime *rt, const std::string &instance_name, pmt_t user_arg) - : mb_mblock(rt, instance_name, user_arg) + : mb_mblock(rt, instance_name, user_arg), + d_fake_rx(false) { - // define our ports + // Dictionary for arguments to all of the components + pmt_t usrp_dict = user_arg; + // 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)); + + 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)); } - // FIXME ... initializing to 2 channels on each for now, eventually we should - // query the FPGA to get these values - d_ntx_chan = 2; + define_component("usrp", "usrp_usb_interface", 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; +} + +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() @@ -119,78 +138,328 @@ 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 metadata = msg->metadata(); pmt_t invocation_handle; - pmt_t reply_data; + pmt_t metadata = msg->metadata(); pmt_t status; - if (1){ + 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) + // 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. - if (pmt_eq(port_id, d_cs->port_symbol())){ // message came in on our control/status port + //---------------- 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)); - if (pmt_eq(event, s_cmd_open)){ - // extract args from data - invocation_handle = pmt_nth(0, data); - long which_usrp = pmt_to_long(pmt_nth(1, data)); // integer usrp id, usually 0 + 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(); + } - // Do the right thing.... - // build a reply - (void) which_usrp; // avoid unused warning + 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 == 0x1f) + 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)){ - // if everything OK - status = PMT_T; - reply_data = pmt_list2(invocation_handle, status); + // Reject if already open + if(d_opened) { + d_cs->send(s_response_open, pmt_list2(invocation_handle, s_err_usrp_already_opened)); + return; + } - // ...and send it - d_cs->send(s_response_open, reply_data); + // 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)) { - invocation_handle = pmt_nth(0, data); - reply_data = pmt_list2(invocation_handle, pmt_from_long(max_capacity())); - d_cs->send(s_response_max_capacity, reply_data); + + 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)) { - invocation_handle = pmt_nth(0, data); - reply_data = pmt_list2(invocation_handle, pmt_from_long(d_ntx_chan)); - d_cs->send(s_response_ntx_chan, reply_data); + + 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)) { - invocation_handle = pmt_nth(0, data); - reply_data = pmt_list2(invocation_handle, pmt_from_long(d_nrx_chan)); - d_cs->send(s_response_nrx_chan, reply_data); - } + + 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)) { - invocation_handle = pmt_nth(0, data); - reply_data = pmt_list2(invocation_handle, pmt_from_long(current_capacity_allocation())); - d_cs->send(s_response_current_capacity_allocation, reply_data); + + 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; + } - if (pmt_eq(event, s_cmd_allocate_channel)){ - handle_cmd_allocate_channel(port_id, data); - 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 (pmt_eq(event, s_cmd_deallocate_channel)) { - handle_cmd_deallocate_channel(port_id, data); - return; + 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) { - if (pmt_eq(event, s_cmd_xmit_raw_frame)){ - handle_cmd_xmit_raw_frame(data); - return; + //------------ 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: @@ -231,128 +500,94 @@ long usrp_server::current_capacity_allocation() { return capacity; } -void usrp_server::handle_cmd_allocate_channel(pmt_t port_id, pmt_t data) { - +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, port; - pmt_t reply_data; + long chan; - // If it's a TX port, allocate on a free channel, else check if it's a RX port - // and allocate. - if((port = tx_port_index(port_id)) != -1) { + // Check capacity exists + if((D_USB_CAPACITY - current_capacity_allocation()) < rqstd_capacity) { - // Check capacity exists - if((D_USB_CAPACITY - current_capacity_allocation()) < rqstd_capacity) { - reply_data = pmt_list3(invocation_handle, pmt_from_long(RQSTD_CAPACITY_UNAVAIL), PMT_NIL); // no capacity available - d_tx[port]->send(s_response_allocate_channel, reply_data); - return; - } + // 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 < d_ntx_chan; chan++) { - if(d_chaninfo_tx[chan].owner == PMT_NIL) { - d_chaninfo_tx[chan].owner = port_id; - d_chaninfo_tx[chan].assigned_capacity = rqstd_capacity; - reply_data = pmt_list3(invocation_handle, PMT_T, pmt_from_long(chan)); - d_tx[port]->send(s_response_allocate_channel, reply_data); - return; - } - } + // Find a free channel, assign the capacity and respond + for(chan=0; chan < (long)chan_info.size(); chan++) { - std::cout << "[USRP_SERVER] Couldnt find a TX chan\n"; + if(verbose) + std::cout << "[USRP_SERVER] Checking chan: " << chan + << " owner " << chan_info[chan].owner + << " size " << chan_info.size() + << std::endl; - reply_data = pmt_list3(invocation_handle, pmt_from_long(CHANNEL_UNAVAIL), PMT_NIL); // no free TX chan found - d_tx[port]->send(s_response_allocate_channel, reply_data); - return; - } + if(chan_info[chan].owner == PMT_NIL) { - // Repeat the same process on the RX side if the port was not determined to be TX - if((port = rx_port_index(port_id)) != -1) { - - if((D_USB_CAPACITY - current_capacity_allocation()) < rqstd_capacity) { - reply_data = pmt_list3(invocation_handle, pmt_from_long(RQSTD_CAPACITY_UNAVAIL), PMT_NIL); // no capacity available - d_rx[port]->send(s_response_allocate_channel, reply_data); + 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; } + + } - for(chan=0; chan < d_nrx_chan; chan++) { - if(d_chaninfo_rx[chan].owner == PMT_NIL) { - d_chaninfo_rx[chan].owner = port_id; - d_chaninfo_rx[chan].assigned_capacity = rqstd_capacity; - reply_data = pmt_list3(invocation_handle, PMT_T, pmt_from_long(chan)); - d_rx[port]->send(s_response_allocate_channel, reply_data); - return; - } - } + if (verbose) + std::cout << "[USRP_SERVER] Couldnt find a TX chan\n"; - std::cout << "[USRP_SERVER] Couldnt find a RX chan\n"; - reply_data = pmt_list3(invocation_handle, pmt_from_long(CHANNEL_UNAVAIL), PMT_NIL); // no free RX chan found - d_rx[port]->send(s_response_allocate_channel, reply_data); - return; - } + // no free TX chan found + port->send(s_response_allocate_channel, + pmt_list3(invocation_handle, + s_err_channel_unavailable, + PMT_NIL)); + return; } // Check the port type and deallocate assigned capacity based on this, ensuring -// that the owner of the method invocation is the owner of the port and that -// the channel number is valid. -void usrp_server::handle_cmd_deallocate_channel(pmt_t port_id, pmt_t data) { +// that the owner of the method invocation is the owner of the port and that the +// channel number is valid. +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)); - long port; - pmt_t reply_data; - - // Check that the channel number is valid, and that the calling port is the owner - // of the channel, and if so remove the assigned capacity. - if((port = tx_port_index(port_id)) != -1) { - - if(channel >= d_ntx_chan) { - reply_data = pmt_list2(invocation_handle, pmt_from_long(CHANNEL_INVALID)); // not a legit channel number - d_tx[port]->send(s_response_deallocate_channel, reply_data); - return; - } - if(d_chaninfo_tx[channel].owner != port_id) { - reply_data = pmt_list2(invocation_handle, pmt_from_long(PERMISSION_DENIED)); // not the owner of the port - d_tx[port]->send(s_response_deallocate_channel, reply_data); - return; - } - - d_chaninfo_tx[channel].assigned_capacity = 0; - d_chaninfo_tx[channel].owner = PMT_NIL; - - reply_data = pmt_list2(invocation_handle, PMT_T); - d_tx[port]->send(s_response_deallocate_channel, reply_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; - } - - // Repeated process on the RX side - if((port = rx_port_index(port_id)) != -1) { - if(channel >= d_nrx_chan) { - reply_data = pmt_list2(invocation_handle, pmt_from_long(CHANNEL_INVALID)); // not a legit channel number - d_rx[port]->send(s_response_deallocate_channel, reply_data); - return; - } - - if(d_chaninfo_rx[channel].owner != port_id) { - reply_data = pmt_list2(invocation_handle, pmt_from_long(PERMISSION_DENIED)); // not the owner of the port - d_rx[port]->send(s_response_deallocate_channel, reply_data); - return; - } - - d_chaninfo_rx[channel].assigned_capacity = 0; - d_chaninfo_rx[channel].owner = PMT_NIL; - - reply_data = pmt_list2(invocation_handle, PMT_T); - d_rx[port]->send(s_response_deallocate_channel, reply_data); - 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; } -void usrp_server::handle_cmd_xmit_raw_frame(pmt_t data) { +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(); @@ -361,10 +596,18 @@ void usrp_server::handle_cmd_xmit_raw_frame(pmt_t 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)); + + // 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; + + // 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)); - // 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 = @@ -372,7 +615,8 @@ void usrp_server::handle_cmd_xmit_raw_frame(pmt_t data) { for(int n=0; n < n_packets; n++) { - long payload_len = std::min((long)(n_bytes-(n*max_payload_len)), (long)max_payload_len); + 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 pkts[n].set_header(pkts[n].FL_START_OF_BURST, channel, 0, payload_len); @@ -382,14 +626,740 @@ void usrp_server::handle_cmd_xmit_raw_frame(pmt_t data) { pkts[n].set_timestamp(0xffffffff); } - memcpy(pkts[n].payload(), (uint8_t *)samples+(max_payload_len * n), payload_len); + 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) + 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 + d_cs_usrp->send(s_cmd_usrp_write, + pmt_list3(invocation_handle, + pmt_from_long(channel), + v_packets)); + + return; +} + +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 = 0x1f; + + // 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_writeable_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_writeable_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_writeable_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; +} + +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; +} + +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; +} + +// Read the packet header, determine the port by the channel owner +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_writeable_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 == 0x1f) { + 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_writeable_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_list5(invocation_handle, + status, + v_samples, + pmt_from_long(pkt->timestamp()), + properties)); + return; +} + +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; + + // 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; + + // 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; + + 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; + + 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; + } +} + +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(); } - pkts[n_packets-1].set_end_of_burst(); // set the last packet's end of burst + // 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; +} + +bool +usrp_server::check_valid(mb_port_sptr port, + long channel, + std::vector<struct channel_info> &chan_info, + pmt_t signal_info) +{ - // interface with the USRP to send the USB packet, since the memory is - // contiguous, this should be a serious of memory copies to the bus, each being - // USB_PKT_SIZE * MAX_PACKET_BURST bytes worth of data (given a full burst) + 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 != 0x1f) { + 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; +} + +// Goes through the vector of RIDs and retreieves an +// available one for use +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; + + return -1; } REGISTER_MBLOCK_CLASS(usrp_server); |