root/ossiedev/branches/june/trunk/platform/USRP_UHD/src/USRP_UHD.cpp @ 11094

/****************************************************************************

Copyright 2005,2006 Virginia Polytechnic Institute and State University

This file is part of the OSSIE USRP_UHD Device.

OSSIE USRP_UHD Device 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 2 of the License, or
(at your option) any later version.

OSSIE USRP_UHD Device 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 OSSIE USRP_UHD Device; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


****************************************************************************/

#include <byteswap.h>
#include <iostream>
#include <vector>
#include <complex>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include <boost/thread.hpp>

#include "ossie/cf.h"
#include "ossie/PortTypes.h"

#include "ossie/ossieSupport.h"
#include "ossie/debug.h"

#include "USRP_UHD.h"

USRP_UHD_i::USRP_UHD_i(char *id, char *label, char *profile) :
        Device_impl(id, label, profile),
        set_rx_packet_count(-1),
        rx_packet_count(1024),
        rx_packet_size(8192),
        rx_data_size(2),
        number_of_channels(1),
        complex(true),
        rx_overruns(0),
        rx_active(false),
        tx_active(false),
        full_duplex(true),
        did_rx(false)
{
// Create USRP_UHD Control ports for TX and RX
    rx_control_port = new USRP_UHD_RX_Control_i(this, "UHD_RX_Control", "DomainName1");
    tx_control_port = new USRP_UHD_TX_Control_i(this, "UHD_TX_Control", "DomainName1");

// Create the ports for TX data
    for(int i=0;i<MAX_TX_CHANNEL;i++)
        tx_data_ports[i] = new standardInterfaces_i::complexShort_p(boost::str(boost::format("UHD_TX_Data_%d") % (i+1)).c_str(), "DomainName1");

// Create the ports for RX Data
    for(int i=0;i<MAX_RX_CHANNEL;i++)
        rx_data_ports[i] = new standardInterfaces_i::complexShort_u(boost::str(boost::format("UHD_RX_Data_%d") % (i+1)).c_str(), "DomainName1");

}

void USRP_UHD_i::start() throw (CF::Resource::StartError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "Start USRP_UHD called")
}

void USRP_UHD_i::stop() throw (CF::Resource::StopError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "Stop USRP_UHD called")
    (rx_active)?std::cout<<"Active": std::cout<<"NOT Active";
}

CORBA::Object_ptr USRP_UHD_i::getPort(const char* portName)
throw(CF::PortSupplier::UnknownPort, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "USRP_UHD getPort called with : " << portName)

    CORBA::Object_var p;

    if (!CORBA::is_nil(p = rx_control_port->getPort(portName)))
        return p._retn();
    else if (!CORBA::is_nil(p = tx_control_port->getPort(portName)))
        return p._retn();
    else {
        for(int i=0;i<MAX_RX_CHANNEL;i++)
                if (!CORBA::is_nil(p = rx_data_ports[i]->getPort(portName)))
                        return p._retn();

        for(int i=0;i<MAX_TX_CHANNEL;i++)
                if (!CORBA::is_nil(p = tx_data_ports[i]->getPort(portName)))
                        return p._retn();
    }

    std::cerr << "Couldn't find port " << portName << "Throwing exception" << std::endl;
   // throw CF::PortSupplier::UnknownPort();
}

void USRP_UHD_i::initialize() throw (CF::LifeCycle::InitializeError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "USRP_UHD Initialize called")
    std::string args("");
    DEBUG(1, USRP_UHD, "Creating the usrp device with: "<< args)
    sdev = uhd::usrp::multi_usrp::make(args);
    dev = sdev->get_device();//uhd::device::make(args);
    probe_usrp();
    rx_control_port->set_number_of_channels(1);
    tx_control_port->set_number_of_channels(1);
    sdev->set_rx_antenna("RX2");
    sdev->set_rx_antenna("TX/RX");
    DEBUG(1, USRP_UHD, "Using Device: "<< sdev->get_pp_string())
}

void USRP_UHD_i::configure(const CF::Properties &configProperties)
throw (CORBA::SystemException, CF::PropertySet::InvalidConfiguration,
       CF::PropertySet::PartialConfiguration)
{

}

void USRP_UHD_i::query(CF::Properties &configProperties)
throw (CORBA::SystemException, CF::UnknownProperties)
{

}


void USRP_UHD_i::releaseObject()
throw (CF::LifeCycle::ReleaseError, CORBA::SystemException)
{

    DEBUG(3, USRP_UHD, "USRP_UHD releaseObject called")
}

void USRP_UHD_i::rx_error_helper(uhd::rx_metadata_t::error_code_t error)
{
    switch(error){
        case uhd::rx_metadata_t::ERROR_CODE_NONE:
            break;
        case uhd::rx_metadata_t::ERROR_CODE_TIMEOUT:
            DEBUG(2, USRP_UHD, "Got timeout before all samples received, possible packet loss")
            break;
        case uhd::rx_metadata_t::ERROR_CODE_LATE_COMMAND:
                DEBUG(2, USRP_UHD, "UHD::ERROR_CODE_LATE_COMMAND")
                break;
        case uhd::rx_metadata_t::ERROR_CODE_BROKEN_CHAIN:
                DEBUG(2, USRP_UHD, "UHD::ERROR_CODE_BROKEN_CHAIN")
                break;
        case uhd::rx_metadata_t::ERROR_CODE_OVERFLOW:
                //DEBUG(2, USRP_UHD, "UHD::ERROR_CODE_OVERFLOW")
                break;
        case uhd::rx_metadata_t::ERROR_CODE_ALIGNMENT:
                DEBUG(2, USRP_UHD, "UHD::ERROR_CODE_ALIGNMENT")
                break;
        case uhd::rx_metadata_t::ERROR_CODE_BAD_PACKET:
                DEBUG(2, USRP_UHD, "UHD::ERROR_CODE_BAD_PACKET")
                break;
        default:
            DEBUG(3, USRP_UHD, "Got Error Code 0x"<<error)
    }
}

void USRP_UHD_i::rx_data_process()
{

///\TODO assumes one channel operation.  Set full_deplex to false to prevent Rx buffering during Tx
///Full duplex mode is still broken. we still cannot transmit and receive simultaneously using WBX board

    //tells the device to stream samples indefinitely
    uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);

    //Use with STREAM_MODE_NUM_SAMPS_AND_DONE/_MORE     
    //stream_cmd.num_samps = 65536;
    //stream_cmd.time_spec = uhd::time_spec_t((time_t)0);
    
    //When true, the device will begin streaming ASAP
    stream_cmd.stream_now = true;

    sdev->issue_stream_cmd(stream_cmd);

    uhd::stream_args_t stream_args("sc16"); //complex floats
    stream_args.args["noclear"] = "1";
    for (size_t chan = 0; chan < rx_channels; chan++)
        stream_args.channels.push_back(chan); //linear mapping
    uhd::rx_streamer::sptr rx_stream = sdev->get_rx_stream(stream_args);
    DEBUG(3, USRP_UHD, "Rx Channels = " << rx_channels)
    PortTypes::ShortSequence I;
    PortTypes::ShortSequence Q;
    
///\TODO Make sure rx_packet_size is >= than USRP_UHD_MIN_RX_SAMPLES!!!!!
    std::vector<std::vector< std::complex<short> > > rx_buff(
                rx_channels, std::vector< std::complex<short> >(rx_packet_size)
    );
    I.length(rx_packet_size);
    Q.length(rx_packet_size);

    std::vector<std::complex<short> *> rx_buff_ptrs;
    for (size_t i = 0; i < rx_buff.size(); i++)
        rx_buff_ptrs.push_back(&rx_buff[i].front());

    DEBUG(3, USRP_UHD, "RX packet count " << rx_packet_count)

    while (rx_active && ((rx_packet_count == -1) || (rx_packet_count > 0))) {
                uhd::rx_metadata_t md;
        //if we are running half duplex mode we will need to tell start streaming at the beginning of the loop
                if (full_duplex == false && did_rx == false)  {
                        uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
                        sdev->issue_stream_cmd(stream_cmd);
                        did_rx = true;
                }
                //DEBUG(9, USRP_UHD, "issue cmd")

        
//TODO get rid of num_rx_samps variable
        size_t num_rx_samps = rx_stream->recv( rx_buff_ptrs, rx_packet_size, md);
                //DEBUG(9, USRP_UHD, "recv data")
        
        //Handle UHD error codes
        rx_error_helper(md.error_code);

        for(int j=0;j<rx_channels;j++){

                        for (unsigned int i = 0; i < num_rx_samps; i++) {
                                I[i] = (CORBA::Short) rx_buff[j][i].real();
                                Q[i] = (CORBA::Short) rx_buff[j][i].imag();
                        }

                        //DEBUG(9, USRP_UHD, "before push packet")
                        rx_data_ports[j]->pushPacket(I, Q);
                        //DEBUG(9, USRP_UHD, "after push packet")
        }



                if (rx_packet_count != -1)
                                --rx_packet_count;
        }

    DEBUG(3, USRP_UHD, "Exiting rx_data_process thread")
    //tells the device to stop streaming samples
    stream_cmd = uhd::stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS;
    sdev->issue_stream_cmd(stream_cmd);
    rx_active = false;
    rx_thread->exit();
}



// USRP_UHD Expects data in little endian format
static inline short convertToLE(short s)
{
#ifdef __powerpc__
    s = bswap_16(s);
#endif

    return s;
}

void USRP_UHD_i::tx_data_process()
{
    PortTypes::ShortSequence *I_in, *Q_in;

    const unsigned int tx_buf_len(USRP_UHD_MAX_TX_SAMPLES);

    uhd::stream_args_t stream_args("sc16"); //complex short
    stream_args.args["noclear"] = "1";
    for (size_t chan = 0; chan < tx_channels; chan++)
        stream_args.channels.push_back(chan); //linear mapping
    uhd::tx_streamer::sptr tx_stream = sdev->get_tx_stream(stream_args);

    uhd::tx_metadata_t md3;

    //Set start and end of burst flags to help with packet fragmentation if needed
    //Host will send ACK packets when an EOB flag is received
    //SOB true -> first packet in sequence gets SOB flag
    //EOB true -> last packet in sequence gets EOB flag
    md3.start_of_burst = false;  
    md3.end_of_burst   = false;     

    //We can send packets in the future if needed
    //md.has_time_spec  = 0;     //no time
    //md.time_spec = uhd::time_spec_t((time_t)0 );


    DEBUG(3, USRP_UHD, "Starting tx_data_process thread.")
    
    tx_active = true; // Cleared to stop TX in USRP_UHD_TX_Control->stop()
    size_t samps_to_send = 0;
    std::vector<std::complex<short> *> tx_buff_ptrs;
    DEBUG(3, USRP_UHD, "Max Number of Sample="<<tx_stream->get_max_num_samps())
    while (tx_active) {

                for(int i=0;i<tx_channels;i++){

                        tx_data_ports[i]->getData(I_in, Q_in);
                        samps_to_send = I_in->length();

                    std::vector<std::complex<short> > tx_buff(samps_to_send);

                        for (unsigned int j = 0; j < samps_to_send; ++j) {
                                tx_buff[j] = std::complex<short int> (convertToLE((*I_in)[j]),convertToLE((*Q_in)[j]));
                        }

                        tx_buff_ptrs.push_back(&tx_buff.front());
                tx_data_ports[i]->bufferEmptied();
                }

        //send the entire packet (driver fragments internally)
        size_t num_tx_samps = tx_stream->send(tx_buff_ptrs, samps_to_send, md3);


                
    }
    DEBUG(3, USRP_UHD, "Exiting tx_data_process thread.")
    tx_thread->exit();
}

void print_tree(const uhd::fs_path &path, uhd::property_tree::sptr tree){
    std::cout << path << std::endl;
    BOOST_FOREACH(const std::string &name, tree->list(path)){
        print_tree(path / name, tree);
    }
}


void USRP_UHD_i::probe_usrp()
{
        DEBUG(3, USRP_UHD, "Probing daughter board of USRP..")
    uhd::property_tree::sptr tree = dev->get_tree();
        uhd::fs_path db_path, mb_path = "/mboards";
        mb_count = tree->list("/mboards").size();
        std::cout<<" The number of MB : "<<mb_count<<std::endl;
        for(int i=0;i<mb_count;i++){
                mb_path = "/mboards";
                mb_path = mb_path / tree->list("/mboards")[i];
                db_count = tree->list( mb_path / "dboards").size();
                std::cout<<"  The number of DB slot: "<<db_count<<std::endl;
                for(int j=0;j<db_count;j++){
                        db_path = mb_path / "dboards" / tree->list( mb_path / "dboards")[j];
                        uhd::usrp::dboard_eeprom_t prom = tree->access<uhd::usrp::dboard_eeprom_t>(db_path / "tx_eeprom").get();
                        std::cout<<"   TxDB ID : "<<prom.id.to_string()<<std::endl;

                        prom = tree->access<uhd::usrp::dboard_eeprom_t>(db_path / "rx_eeprom").get();
                        std::cout<<"   RxDB ID : "<<prom.id.to_string()<<std::endl;
                }


        }

}